Merge changes I04cede20,Ie54cd037,I6756d4ba,I6a01afa5 into main
* changes:
Prep lqr_plot for plotting swerve
Provide a goal to our SAC implementation
Split Soft Actor-Criticl problem out into a class
Add plotly and tensorflow_probability
diff --git a/y2024_bot3/BUILD b/y2024_bot3/BUILD
new file mode 100644
index 0000000..c766a54
--- /dev/null
+++ b/y2024_bot3/BUILD
@@ -0,0 +1,309 @@
+load("//aos:config.bzl", "aos_config")
+load("//aos/util:config_validator_macro.bzl", "config_validator_test")
+load("//frc971:downloader.bzl", "robot_downloader")
+
+config_validator_test(
+ name = "config_validator_test",
+ config = "//y2024_bot3:aos_config",
+)
+
+robot_downloader(
+ binaries = [
+ "//aos/network:web_proxy_main",
+ "//aos/events/logging:log_cat",
+ "//y2024_bot3/constants:constants_sender",
+ "//aos/events:aos_timing_report_streamer",
+ ],
+ data = [
+ ":aos_config",
+ "//aos/starter:roborio_irq_config.json",
+ "//y2024_bot3/constants:constants.json",
+ "@ctre_phoenix6_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix6_tools_athena//:shared_libraries",
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ dirs = [
+ "//y2024_bot3/www:www_files",
+ ],
+ start_binaries = [
+ "//aos/events/logging:logger_main",
+ "//aos/network:web_proxy_main",
+ "//aos/starter:irq_affinity",
+ ":joystick_reader",
+ ":wpilib_interface",
+ "//frc971/can_logger",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//y2024_bot3/control_loops/superstructure:superstructure",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
+ name = "orin_download",
+ binaries = [
+ "//frc971/wpilib:joystick_republish",
+ "//aos/events:aos_timing_report_streamer",
+ "//aos/events/logging:log_cat",
+ "//aos:aos_jitter",
+ "//aos/network:web_proxy_main",
+ "//aos/starter:irq_affinity",
+ "//aos/util:foxglove_websocket",
+ "//frc971/image_streamer:image_streamer",
+ "//frc971/orin:hardware_monitor",
+ "//frc971/orin:argus_monitor",
+ "//frc971/vision:intrinsics_calibration",
+ "//aos/util:filesystem_monitor",
+ "//y2024_bot3/vision:viewer",
+ "//y2024_bot3/constants:constants_sender",
+ "//frc971/orin:localizer_logger",
+ "//frc971/vision:foxglove_image_converter",
+ ],
+ data = [
+ ":aos_config",
+ "//frc971/orin:orin_irq_config.json",
+ "//y2024_bot3/constants:constants.json",
+ "//y2024_bot3/vision:image_streamer_start",
+ "//y2024_bot3/www:www_files",
+ ],
+ dirs = [
+ "//y2024_bot3/www:www_files",
+ "//frc971/image_streamer/www:www_files",
+ ],
+ start_binaries = [
+ "//aos/events/logging:logger_main",
+ "//frc971/imu_fdcan:can_translator",
+ "//frc971/imu_fdcan:dual_imu_blender",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//aos/network:web_proxy_main",
+ "//aos/starter:irq_affinity",
+ "//frc971/orin:argus_camera",
+ "//y2024_bot3/orin:can_logger",
+ "//y2024_bot3/vision:apriltag_detector",
+ "//frc971/vision:image_logger",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
+ target_type = "pi",
+)
+
+aos_config(
+ name = "aos_config",
+ src = "y2024_bot3.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//frc971/input:robot_state_fbs",
+ "//frc971/vision:vision_fbs",
+ "//frc971/vision:target_map_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":config_imu",
+ ":config_orin1",
+ ":config_roborio",
+ ],
+)
+
+aos_config(
+ name = "config_imu",
+ src = "y2024_bot3_imu.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/imu_fdcan:dual_imu_fbs",
+ "//frc971/imu_fdcan:can_translator_status_fbs",
+ "//frc971/imu_fdcan:dual_imu_blender_status_fbs",
+ "//y2024_bot3/constants:constants_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/can_logger:can_logging_fbs",
+ "//frc971/orin:hardware_stats_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/util:filesystem_fbs",
+ "//aos/network:remote_message_fbs",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:target_map_fbs",
+ "//frc971/vision:vision_fbs",
+ "@com_github_foxglove_schemas//:schemas",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ ],
+)
+
+aos_config(
+ name = "config_roborio",
+ src = "y2024_bot3_roborio.json",
+ flatbuffers = [
+ "//frc971:can_configuration_fbs",
+ "//aos/network:remote_message_fbs",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/wpilib:pdp_values_fbs",
+ "//y2024_bot3/constants:constants_fbs",
+ "//aos/network:timestamp_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_goal_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_can_position_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_output_fbs",
+ "//frc971/control_loops/drivetrain:rio_localizer_inputs_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_position_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_status_fbs",
+ "//frc971/can_logger:can_logging_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ "//frc971/input:aos_config",
+ ],
+)
+
+aos_config(
+ name = "config_orin1",
+ src = "y2024_bot3_orin1.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_fbs",
+ "//y2024_bot3/constants:constants_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/orin:hardware_stats_fbs",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:target_map_fbs",
+ "//frc971/vision:vision_fbs",
+ "//aos/util:filesystem_fbs",
+ "@com_github_foxglove_schemas//:schemas",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ "//frc971/input:aos_config",
+ ],
+)
+
+cc_library(
+ name = "constants",
+ srcs = [
+ "constants.cc",
+ ],
+ hdrs = [
+ "constants.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/mutex",
+ "//aos/network:team_number",
+ "//frc971:constants",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
+ "//y2024_bot3/constants:constants_fbs",
+ "@com_google_absl//absl/base",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
+ ],
+)
+
+cc_binary(
+ name = "wpilib_interface",
+ srcs = [
+ "wpilib_interface.cc",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":constants",
+ "//aos:init",
+ "//aos:math",
+ "//aos/containers:sized_array",
+ "//aos/events:shm_event_loop",
+ "//aos/logging",
+ "//aos/stl_mutex",
+ "//aos/time",
+ "//aos/util:log_interval",
+ "//aos/util:phased_loop",
+ "//aos/util:wrapping_counter",
+ "//frc971:can_configuration_fbs",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
+ "//frc971/input:robot_state_fbs",
+ "//frc971/queues:gyro_fbs",
+ "//frc971/wpilib:ADIS16448",
+ "//frc971/wpilib:buffered_pcm",
+ "//frc971/wpilib:can_drivetrain_writer",
+ "//frc971/wpilib:can_sensor_reader",
+ "//frc971/wpilib:dma",
+ "//frc971/wpilib:drivetrain_writer",
+ "//frc971/wpilib:encoder_and_potentiometer",
+ "//frc971/wpilib:generic_can_writer",
+ "//frc971/wpilib:joystick_sender",
+ "//frc971/wpilib:logging_fbs",
+ "//frc971/wpilib:pdp_fetcher",
+ "//frc971/wpilib:sensor_reader",
+ "//frc971/wpilib:talonfx",
+ "//frc971/wpilib:wpilib_robot_base",
+ "//third_party:phoenix",
+ "//third_party:phoenix6",
+ "//third_party:wpilib",
+ "//y2024_bot3/constants:constants_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_can_position_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_output_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_position_fbs",
+ ],
+)
+
+cc_binary(
+ name = "joystick_reader",
+ srcs = [
+ ":joystick_reader.cc",
+ ],
+ deps = [
+ ":constants",
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/logging",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//frc971/input:action_joystick_input",
+ "//frc971/input:drivetrain_input",
+ "//frc971/input:joystick_input",
+ "//frc971/input:redundant_joystick_data",
+ "//y2024_bot3/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+sh_binary(
+ name = "log_web_proxy",
+ srcs = ["log_web_proxy.sh"],
+ data = [
+ ":aos_config",
+ "//aos/network:log_web_proxy_main",
+ "//frc971/www:starter_main_bundle.min.js",
+ "//y2024_bot3/www:field_main_bundle.min.js",
+ "//y2024_bot3/www:files",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
diff --git a/y2024_bot3/__init__.py b/y2024_bot3/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024_bot3/__init__.py
diff --git a/y2024_bot3/constants.cc b/y2024_bot3/constants.cc
new file mode 100644
index 0000000..9b97566
--- /dev/null
+++ b/y2024_bot3/constants.cc
@@ -0,0 +1,41 @@
+#include "y2024_bot3/constants.h"
+
+#include <cinttypes>
+#include <map>
+
+#if __has_feature(address_sanitizer)
+#include "sanitizer/lsan_interface.h"
+#endif
+
+#include "absl/base/call_once.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
+
+#include "aos/mutex/mutex.h"
+#include "aos/network/team_number.h"
+
+namespace y2024_bot3::constants {
+
+Values MakeValues(uint16_t team) {
+ LOG(INFO) << "creating a Constants for team: " << team;
+
+ Values r;
+
+ switch (team) {
+ // A set of constants for tests.
+ case 1:
+ break;
+
+ case kThirdRobotTeamNumber:
+ break;
+
+ default:
+ LOG(FATAL) << "unknown team: " << team;
+ }
+
+ return r;
+}
+
+Values MakeValues() { return MakeValues(aos::network::GetTeamNumber()); }
+
+} // namespace y2024_bot3::constants
diff --git a/y2024_bot3/constants.h b/y2024_bot3/constants.h
new file mode 100644
index 0000000..31d104f
--- /dev/null
+++ b/y2024_bot3/constants.h
@@ -0,0 +1,53 @@
+#ifndef Y2024_BOT3_CONSTANTS_H_
+#define Y2024_BOT3_CONSTANTS_H_
+
+#include <array>
+#include <cmath>
+#include <cstdint>
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+#include "y2024_bot3/constants/constants_generated.h"
+
+namespace y2024_bot3::constants {
+
+constexpr uint16_t kThirdRobotTeamNumber = 9971;
+
+struct Values {
+ static const int kSuperstructureCANWriterPriority = 35;
+
+ struct PotAndAbsEncoderConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ subsystem_params;
+ double potentiometer_offset;
+ };
+
+ struct AbsoluteEncoderConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ subsystem_params;
+ };
+
+ struct PotConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::RelativeEncoderZeroingEstimator>
+ subsystem_params;
+ double potentiometer_offset;
+ };
+};
+
+// Creates and returns a Values instance for the constants.
+// Should be called before realtime because this allocates memory.
+// Only the first call to either of these will be used.
+constants::Values MakeValues(uint16_t team);
+
+// Calls MakeValues with aos::network::GetTeamNumber()
+constants::Values MakeValues();
+
+} // namespace y2024_bot3::constants
+
+#endif // Y2024_BOT3_CONSTANTS_H_
diff --git a/y2024_bot3/constants/9971.json b/y2024_bot3/constants/9971.json
new file mode 100644
index 0000000..5583b44
--- /dev/null
+++ b/y2024_bot3/constants/9971.json
@@ -0,0 +1,18 @@
+{
+ "cameras": [
+ {
+ "calibration": {% include 'y2024_bot3/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_bot3/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_bot3/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json' %}
+ },
+ {
+ "calibration": {% include 'y2024_bot3/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json' %}
+ }
+ ],
+ "robot": {},
+ {% include 'y2024_bot3/constants/common.json' %}
+}
diff --git a/y2024_bot3/constants/BUILD b/y2024_bot3/constants/BUILD
new file mode 100644
index 0000000..1ce69a0
--- /dev/null
+++ b/y2024_bot3/constants/BUILD
@@ -0,0 +1,106 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("//tools/build_rules:template.bzl", "jinja2_template")
+load("//y2024_bot3/constants:validator.bzl", "constants_json")
+
+cc_library(
+ name = "simulated_constants_sender",
+ srcs = ["simulated_constants_sender.cc"],
+ hdrs = ["simulated_constants_sender.h"],
+ data = [":test_constants.json"],
+ visibility = ["//y2024_bot3:__subpackages__"],
+ deps = [
+ ":constants_fbs",
+ ":constants_list_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:path",
+ "//frc971/constants:constants_sender_lib",
+ ],
+)
+
+jinja2_template(
+ name = "test_constants_unvalidated.json",
+ src = "test_constants.jinja2.json",
+ includes = glob([
+ "test_data/*.json",
+ ]) + [
+ "common.jinja2",
+ "common.json",
+ "//y2024_bot3/constants/calib_files",
+ "//y2024_bot3/vision/maps",
+ ],
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+jinja2_template(
+ name = "constants_unvalidated.json",
+ src = "constants.jinja2.json",
+ includes = [
+ "9971.json",
+ "common.jinja2",
+ "common.json",
+ "//y2024_bot3/constants/calib_files",
+ "//y2024_bot3/vision/maps",
+ ],
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+static_flatbuffer(
+ name = "constants_fbs",
+ srcs = ["constants.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:target_map_fbs",
+ "//frc971/zeroing:constants_fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "constants_list_fbs",
+ srcs = ["constants_list.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [":constants_fbs"],
+)
+
+cc_binary(
+ name = "constants_sender",
+ srcs = ["constants_sender.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":constants_fbs",
+ ":constants_list_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/testing:path",
+ "//frc971/constants:constants_sender_lib",
+ ],
+)
+
+cc_binary(
+ name = "constants_formatter",
+ srcs = ["constants_formatter.cc"],
+ data = [":constants_unvalidated.json"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":constants_list_fbs",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
+ ],
+)
+
+constants_json(
+ name = "constants_json",
+ src = ":constants_unvalidated.json",
+ out = "constants.json",
+)
+
+constants_json(
+ name = "test_constants_json",
+ src = ":test_constants_unvalidated.json",
+ out = "test_constants.json",
+)
diff --git a/y2024_bot3/constants/calib_files/BUILD b/y2024_bot3/constants/calib_files/BUILD
new file mode 100644
index 0000000..1f33d50
--- /dev/null
+++ b/y2024_bot3/constants/calib_files/BUILD
@@ -0,0 +1,5 @@
+filegroup(
+ name = "calib_files",
+ srcs = glob(["*.json"]),
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024_bot3/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json b/y2024_bot3/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json
new file mode 100755
index 0000000..e5f50e3
--- /dev/null
+++ b/y2024_bot3/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "imu",
+ "team_number": 9971,
+ "intrinsics": [
+ 646.04834,
+ 0.0,
+ 703.327576,
+ 0.0,
+ 645.444458,
+ 527.86261,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ -0.258819,
+ -0.965926,
+ -0.323293,
+ 1.0,
+ 0.0,
+ -0.0,
+ 0.268249,
+ 0.0,
+ -0.965926,
+ 0.258819,
+ 0.471129,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.251594,
+ 0.064935,
+ 0.000479,
+ 0.000036,
+ -0.007207
+ ],
+ "calibration_timestamp": 1708821845975708672,
+ "camera_id": "24-10",
+ "camera_number": 0,
+ "reprojection_error": 1.523209
+}
diff --git a/y2024_bot3/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json b/y2024_bot3/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json
new file mode 100755
index 0000000..530e88a
--- /dev/null
+++ b/y2024_bot3/constants/calib_files/calibration_imu-9971-1_cam-24-12_2024-03-24_11-52-49.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "imu",
+ "team_number": 9971,
+ "intrinsics": [
+ 647.19928,
+ 0.0,
+ 690.698181,
+ 0.0,
+ 646.449158,
+ 530.162842,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.99969,
+ 0.020217,
+ -0.014527,
+ 0.160342,
+ 0.009501,
+ 0.229548,
+ 0.973251,
+ 0.25208,
+ 0.023011,
+ -0.973088,
+ 0.229284,
+ 0.41504,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.249799,
+ 0.062593,
+ 0.00003,
+ 0.000366,
+ -0.006532
+ ],
+ "calibration_timestamp": 1711306369592332476,
+ "camera_id": "24-12",
+ "camera_number": 1,
+ "reprojection_error": 1.23409
+}
\ No newline at end of file
diff --git a/y2024_bot3/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json b/y2024_bot3/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json
new file mode 100755
index 0000000..97234c9
--- /dev/null
+++ b/y2024_bot3/constants/calib_files/calibration_orin1-9971-0_cam-24-09_2024-03-24_11-52-49.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "team_number": 9971,
+ "intrinsics": [
+ 648.187805,
+ 0.0,
+ 736.903137,
+ 0.0,
+ 648.028687,
+ 557.169861,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -0.999204,
+ -0.034711,
+ -0.019682,
+ 0.162103,
+ 0.028118,
+ -0.262536,
+ -0.964512,
+ -0.329348,
+ 0.028312,
+ -0.964298,
+ 0.263303,
+ 0.562319,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.265564,
+ 0.078084,
+ -0.000231,
+ 0.000386,
+ -0.010425
+ ],
+ "calibration_timestamp": 1711306369593360533,
+ "camera_id": "24-09",
+ "camera_number": 0,
+ "reprojection_error": 1.881098
+}
\ No newline at end of file
diff --git a/y2024_bot3/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json b/y2024_bot3/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json
new file mode 100755
index 0000000..daef89c
--- /dev/null
+++ b/y2024_bot3/constants/calib_files/calibration_orin1-9971-1_cam-24-11_2024-03-24_11-52-49.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "team_number": 9971,
+ "intrinsics": [
+ 649.866699,
+ 0.0,
+ 709.355713,
+ 0.0,
+ 648.893066,
+ 576.101868,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -0.014623,
+ 0.004459,
+ 0.999883,
+ 0.345385,
+ 0.997965,
+ 0.062137,
+ 0.014318,
+ 0.150131,
+ -0.062066,
+ 0.998058,
+ -0.005359,
+ 0.570236,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.248092,
+ 0.060938,
+ 0.000313,
+ 0.00009,
+ -0.006163
+ ],
+ "calibration_timestamp": 1711306369592886702,
+ "camera_id": "24-11",
+ "camera_number": 1,
+ "reprojection_error": 1.450069
+}
\ No newline at end of file
diff --git a/y2024_bot3/constants/common.jinja2 b/y2024_bot3/constants/common.jinja2
new file mode 100644
index 0000000..9b7af82
--- /dev/null
+++ b/y2024_bot3/constants/common.jinja2
@@ -0,0 +1,3 @@
+{% set pi = 3.14159265 %}
+
+{%set zeroing_sample_size = 200 %}
diff --git a/y2024_bot3/constants/common.json b/y2024_bot3/constants/common.json
new file mode 100644
index 0000000..e7b70b4
--- /dev/null
+++ b/y2024_bot3/constants/common.json
@@ -0,0 +1,2 @@
+"common": {
+}
diff --git a/y2024_bot3/constants/constants.fbs b/y2024_bot3/constants/constants.fbs
new file mode 100644
index 0000000..0c7a792
--- /dev/null
+++ b/y2024_bot3/constants/constants.fbs
@@ -0,0 +1,27 @@
+include "frc971/vision/calibration.fbs";
+include "frc971/vision/target_map.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/zeroing/constants.fbs";
+include "frc971/math/matrix.fbs";
+
+namespace y2024_bot3;
+
+table CameraConfiguration {
+ calibration:frc971.vision.calibration.CameraCalibration (id: 0);
+}
+
+table RobotConstants {
+}
+
+// Common table for constants unrelated to the robot
+table Common {
+ target_map:frc971.vision.TargetMap (id: 0);
+}
+
+table Constants {
+ cameras:[CameraConfiguration] (id: 0);
+ robot:RobotConstants (id: 1);
+ common:Common (id: 2);
+}
+
+root_type Constants;
diff --git a/y2024_bot3/constants/constants.jinja2.json b/y2024_bot3/constants/constants.jinja2.json
new file mode 100644
index 0000000..63fad42
--- /dev/null
+++ b/y2024_bot3/constants/constants.jinja2.json
@@ -0,0 +1,8 @@
+{
+ "constants": [
+ {
+ "team": 9971,
+ "data": {% include 'y2024_bot3/constants/9971.json' %}
+ }
+ ]
+}
diff --git a/y2024_bot3/constants/constants_formatter.cc b/y2024_bot3/constants/constants_formatter.cc
new file mode 100644
index 0000000..4b2a601
--- /dev/null
+++ b/y2024_bot3/constants/constants_formatter.cc
@@ -0,0 +1,27 @@
+#include "absl/log/check.h"
+#include "absl/log/log.h"
+
+#include "aos/flatbuffers.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/util/file.h"
+#include "y2024_bot3/constants/constants_list_generated.h"
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ CHECK(argc == 3) << ": Expected input and output json files to be passed in.";
+
+ aos::FlatbufferDetachedBuffer<y2024_bot3::ConstantsList> constants =
+ aos::JsonFileToFlatbuffer<y2024_bot3::ConstantsList>(argv[1]);
+
+ // Make sure the file is valid json before we output a formatted version.
+ CHECK(constants.message().constants() != nullptr)
+ << ": Failed to parse " << std::string(argv[2]);
+
+ aos::util::WriteStringToFileOrDie(
+ std::string(argv[2]),
+ aos::FlatbufferToJson(constants, {.multi_line = true}));
+
+ return 0;
+}
diff --git a/y2024_bot3/constants/constants_list.fbs b/y2024_bot3/constants/constants_list.fbs
new file mode 100644
index 0000000..af2b49c
--- /dev/null
+++ b/y2024_bot3/constants/constants_list.fbs
@@ -0,0 +1,14 @@
+include "y2024_bot3/constants/constants.fbs";
+
+namespace y2024_bot3;
+
+table TeamAndConstants {
+ team:long (id: 0);
+ data:Constants (id: 1);
+}
+
+table ConstantsList {
+ constants:[TeamAndConstants] (id: 0);
+}
+
+root_type ConstantsList;
diff --git a/y2024_bot3/constants/constants_sender.cc b/y2024_bot3/constants/constants_sender.cc
new file mode 100644
index 0000000..d74f0d0
--- /dev/null
+++ b/y2024_bot3/constants/constants_sender.cc
@@ -0,0 +1,25 @@
+#include "absl/flags/flag.h"
+
+#include "aos/configuration.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "y2024_bot3/constants/constants_generated.h"
+#include "y2024_bot3/constants/constants_list_generated.h"
+
+ABSL_FLAG(std::string, config, "aos_config.json", "Path to the AOS config.");
+ABSL_FLAG(std::string, constants_path, "constants.json",
+ "Path to the constant file");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(absl::GetFlag(FLAGS_config));
+ aos::ShmEventLoop event_loop(&config.message());
+ frc971::constants::ConstantSender<y2024_bot3::Constants,
+ y2024_bot3::ConstantsList>
+ constants_sender(&event_loop, absl::GetFlag(FLAGS_constants_path));
+ // Don't need to call Run().
+ return 0;
+}
diff --git a/y2024_bot3/constants/simulated_constants_sender.cc b/y2024_bot3/constants/simulated_constants_sender.cc
new file mode 100644
index 0000000..77d818a
--- /dev/null
+++ b/y2024_bot3/constants/simulated_constants_sender.cc
@@ -0,0 +1,23 @@
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "y2024_bot3/constants/constants_generated.h"
+#include "y2024_bot3/constants/constants_list_generated.h"
+
+namespace y2024_bot3 {
+bool SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path,
+ const std::set<std::string_view> &node_names) {
+ for (const aos::Node *node : factory->nodes()) {
+ if (!node_names.empty() &&
+ !node_names.contains(node->name()->string_view())) {
+ continue;
+ }
+ std::unique_ptr<aos::EventLoop> event_loop =
+ factory->MakeEventLoop("constants_sender", node);
+ frc971::constants::ConstantSender<Constants, ConstantsList> sender(
+ event_loop.get(), constants_path, team, "/constants");
+ }
+ return true;
+}
+} // namespace y2024_bot3
diff --git a/y2024_bot3/constants/simulated_constants_sender.h b/y2024_bot3/constants/simulated_constants_sender.h
new file mode 100644
index 0000000..90b3724
--- /dev/null
+++ b/y2024_bot3/constants/simulated_constants_sender.h
@@ -0,0 +1,21 @@
+#ifndef Y2024_BOT3_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+#define Y2024_BOT3_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+
+#include <set>
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+
+namespace y2024_bot3 {
+// Returns true, to allow this to be easily called in the initializer list of a
+// constructor.
+// If node_names is specified, we limit ourselves to sending constants on the
+// specified nodes.
+bool SendSimulationConstants(
+ aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path =
+ aos::testing::ArtifactPath("y2024_bot3/constants/test_constants.json"),
+ const std::set<std::string_view> &node_names = {});
+} // namespace y2024_bot3
+
+#endif // Y2024_BOT3_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2024_bot3/constants/test_constants.jinja2.json b/y2024_bot3/constants/test_constants.jinja2.json
new file mode 100644
index 0000000..fdb764a
--- /dev/null
+++ b/y2024_bot3/constants/test_constants.jinja2.json
@@ -0,0 +1,8 @@
+{
+ "constants": [
+ {
+ "team": 9971,
+ "data": {% include 'y2024_bot3/constants/test_data/test_team.json' %}
+ }
+ ]
+}
diff --git a/y2024_bot3/constants/test_data/test_team.json b/y2024_bot3/constants/test_data/test_team.json
new file mode 100644
index 0000000..deacef4
--- /dev/null
+++ b/y2024_bot3/constants/test_data/test_team.json
@@ -0,0 +1,4 @@
+{
+ "robot": {},
+ {% include 'y2024_bot3/constants/common.json' %}
+}
diff --git a/y2024_bot3/constants/validator.bzl b/y2024_bot3/constants/validator.bzl
new file mode 100644
index 0000000..7224e13
--- /dev/null
+++ b/y2024_bot3/constants/validator.bzl
@@ -0,0 +1,13 @@
+load("@aspect_bazel_lib//lib:run_binary.bzl", "run_binary")
+
+# Validates the constants.json file and outputs a formatted version.
+# TODO(max): Make this generic/template it out into frc971
+def constants_json(name, src, out):
+ run_binary(
+ name = name,
+ tool = "//y2024_bot3/constants:constants_formatter",
+ srcs = [src],
+ outs = [out],
+ args = ["$(location %s)" % (src)] + ["$(location %s)" % (out)],
+ visibility = ["//visibility:public"],
+ )
diff --git a/y2024_bot3/control_loops/BUILD b/y2024_bot3/control_loops/BUILD
new file mode 100644
index 0000000..55c0974
--- /dev/null
+++ b/y2024_bot3/control_loops/BUILD
@@ -0,0 +1,6 @@
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2024_bot3:python_init"],
+)
diff --git a/y2024_bot3/control_loops/__init__.py b/y2024_bot3/control_loops/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024_bot3/control_loops/__init__.py
diff --git a/y2024_bot3/control_loops/python/BUILD b/y2024_bot3/control_loops/python/BUILD
new file mode 100644
index 0000000..9ec6b57
--- /dev/null
+++ b/y2024_bot3/control_loops/python/BUILD
@@ -0,0 +1,8 @@
+package(default_visibility = ["//y2024_bot3:__subpackages__"])
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2024_bot3/control_loops:python_init"],
+)
diff --git a/y2024_bot3/control_loops/python/__init__.py b/y2024_bot3/control_loops/python/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024_bot3/control_loops/python/__init__.py
diff --git a/y2024_bot3/control_loops/superstructure/BUILD b/y2024_bot3/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..8d909bf
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/BUILD
@@ -0,0 +1,170 @@
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("//tools/build_rules:js.bzl", "ts_project")
+
+package(default_visibility = ["//visibility:public"])
+
+static_flatbuffer(
+ name = "superstructure_goal_fbs",
+ srcs = [
+ "superstructure_goal.fbs",
+ ],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ ],
+)
+
+flatbuffer_ts_library(
+ name = "superstructure_status_ts_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ deps = [
+ "//frc971/control_loops:control_loops_ts_fbs",
+ "//frc971/control_loops:profiled_subsystem_ts_fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ ],
+)
+
+flatbuffer_ts_library(
+ name = "superstructure_position_ts_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ deps = [
+ "//frc971/control_loops:control_loops_ts_fbs",
+ "//frc971/control_loops:profiled_subsystem_ts_fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "superstructure_can_position_fbs",
+ srcs = ["superstructure_can_position.fbs"],
+ deps = ["//frc971/control_loops:can_talonfx_fbs"],
+)
+
+flatbuffer_ts_library(
+ name = "superstructure_can_position_ts_fbs",
+ srcs = ["superstructure_can_position.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = ["//frc971/control_loops:can_talonfx_ts_fbs"],
+)
+
+cc_library(
+ name = "superstructure_lib",
+ srcs = [
+ "superstructure.cc",
+ ],
+ hdrs = [
+ "superstructure.h",
+ ],
+ data = [],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos:flatbuffer_merge",
+ "//aos/events:event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:control_loop",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
+ "//y2024_bot3:constants",
+ "//y2024_bot3/constants:constants_fbs",
+ "//y2024_bot3/constants:simulated_constants_sender",
+ ],
+)
+
+cc_binary(
+ name = "superstructure",
+ srcs = [
+ "superstructure_main.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
+
+cc_test(
+ name = "superstructure_lib_test",
+ srcs = [
+ "superstructure_lib_test.cc",
+ ],
+ data = [
+ "//y2024_bot3:aos_config",
+ ],
+ deps = [
+ ":superstructure_can_position_fbs",
+ ":superstructure_goal_fbs",
+ ":superstructure_lib",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos:json_to_flatbuffer",
+ "//aos:math",
+ "//aos/events/logging:log_writer",
+ "//aos/testing:googletest",
+ "//aos/time",
+ "//frc971/control_loops:capped_test_plant",
+ "//frc971/control_loops:control_loop_test",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:subsystem_simulator",
+ "//frc971/control_loops:team_number_test_environment",
+ ],
+)
+
+cc_binary(
+ name = "superstructure_replay",
+ srcs = ["superstructure_replay.cc"],
+ deps = [
+ ":superstructure_lib",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/network:team_number",
+ ],
+)
+
+ts_project(
+ name = "superstructure_plotter",
+ srcs = ["superstructure_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ ],
+)
diff --git a/y2024_bot3/control_loops/superstructure/superstructure.cc b/y2024_bot3/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..c18da15
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,71 @@
+#include "y2024_bot3/control_loops/superstructure/superstructure.h"
+
+#include <chrono>
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/network/team_number.h"
+#include "aos/time/time.h"
+#include "frc971/zeroing/wrap.h"
+
+ABSL_FLAG(bool, ignore_distance, false,
+ "If true, ignore distance when shooting and obey joystick_reader");
+
+namespace y2024_bot3::control_loops::superstructure {
+
+using ::aos::monotonic_clock;
+
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::RelativeEncoderProfiledJointStatus;
+
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name)
+ : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
+ constants_fetcher_(event_loop),
+ robot_constants_(&constants_fetcher_.constants()),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+ event_loop->SetRuntimeRealtimePriority(30);
+}
+
+bool PositionNear(double position, double goal, double threshold) {
+ return std::abs(position - goal) < threshold;
+}
+
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
+ (void)position;
+
+ if (WasReset()) {
+ AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ }
+
+ OutputT output_struct;
+
+ if (unsafe_goal != nullptr) {
+ }
+
+ if (joystick_state_fetcher_.Fetch() &&
+ joystick_state_fetcher_->has_alliance()) {
+ alliance_ = joystick_state_fetcher_->alliance();
+ }
+
+ if (output) {
+ output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
+ }
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ const bool zeroed = true;
+ const bool estopped = false;
+
+ status_builder.add_zeroed(zeroed);
+ status_builder.add_estopped(estopped);
+
+ (void)status->Send(status_builder.Finish());
+}
+} // namespace y2024_bot3::control_loops::superstructure
diff --git a/y2024_bot3/control_loops/superstructure/superstructure.h b/y2024_bot3/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..6362bc0
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure.h
@@ -0,0 +1,55 @@
+#ifndef Y2024_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2024_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/time/time.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+#include "y2024_bot3/constants.h"
+#include "y2024_bot3/constants/constants_generated.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2024_bot3::control_loops::superstructure {
+
+class Superstructure
+ : public ::frc971::controls::ControlLoop<Goal, Position, Status, Output> {
+ public:
+ using AbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
+
+ double robot_velocity() const;
+
+ protected:
+ virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
+
+ private:
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ const Constants *robot_constants_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+
+ aos::Alliance alliance_ = aos::Alliance::kInvalid;
+
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace y2024_bot3::control_loops::superstructure
+
+#endif // Y2024_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_can_position.fbs b/y2024_bot3/control_loops/superstructure/superstructure_can_position.fbs
new file mode 100644
index 0000000..15a0565
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure_can_position.fbs
@@ -0,0 +1,16 @@
+include "frc971/control_loops/can_talonfx.fbs";
+
+namespace y2024_bot3.control_loops.superstructure;
+
+table CANPosition {
+ // 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: 0, deprecated);
+
+ // The ctre::phoenix::StatusCode of the measurement
+ // Should be OK = 0
+ status:int (id: 1);
+}
+
+root_type CANPosition;
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2024_bot3/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..947f740
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,7 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2024_bot3.control_loops.superstructure;
+
+table Goal {
+}
+root_type Goal;
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..fe26af2
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,243 @@
+#include <chrono>
+#include <memory>
+
+#include "absl/flags/flag.h"
+#include "gtest/gtest.h"
+
+#include "aos/events/logging/log_writer.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/subsystem_simulator.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "y2024_bot3/constants/simulated_constants_sender.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_can_position_generated.h"
+
+ABSL_FLAG(std::string, output_folder, "",
+ "If set, logs all channels to the provided logfile.");
+
+namespace y2024_bot3::control_loops::superstructure::testing {
+
+namespace chrono = std::chrono;
+
+using ::aos::monotonic_clock;
+using ::frc971::CreateProfileParameters;
+using ::frc971::control_loops::CappedTestPlant;
+using ::frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+typedef Superstructure::PotAndAbsoluteEncoderSubsystem
+ PotAndAbsoluteEncoderSubsystem;
+typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
+using PotAndAbsoluteEncoderSimulator =
+ frc971::control_loops::SubsystemSimulator<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
+ PotAndAbsoluteEncoderSubsystem::State,
+ constants::Values::PotAndAbsEncoderConstants>;
+using AbsoluteEncoderSimulator = frc971::control_loops::SubsystemSimulator<
+ frc971::control_loops::AbsoluteEncoderProfiledJointStatus,
+ AbsoluteEncoderSubsystem::State,
+ constants::Values::AbsoluteEncoderConstants>;
+
+class SuperstructureSimulation {
+ public:
+ SuperstructureSimulation(::aos::EventLoop *event_loop,
+ const Constants *simulated_robot_constants,
+ chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ dt_(dt),
+ superstructure_position_sender_(
+ event_loop_->MakeSender<Position>("/superstructure")),
+ superstructure_can_position_sender_(
+ event_loop_->MakeSender<CANPosition>("/superstructure/rio")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<Output>("/superstructure")) {
+ (void)dt_;
+ (void)simulated_robot_constants;
+
+ phased_loop_handle_ = event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
+ }
+
+ // Sends a queue message with the position of the superstructure.
+ void SendPositionMessage() {
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ CHECK_EQ(builder.Send(position_builder.Finish()),
+ aos::RawSender::Error::kOk);
+ }
+
+ private:
+ ::aos::EventLoop *event_loop_;
+ const chrono::nanoseconds dt_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Sender<CANPosition> superstructure_can_position_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+
+ bool first_ = true;
+};
+
+class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
+ public:
+ SuperstructureTest()
+ : ::frc971::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2024_bot3/aos_config.json"),
+ std::chrono::microseconds(5000)),
+ simulated_constants_dummy_(SendSimulationConstants(
+ event_loop_factory(), 9971,
+ "y2024_bot3/constants/test_constants.json")),
+ roborio_(aos::configuration::GetNode(configuration(), "roborio")),
+ logger_pi_(aos::configuration::GetNode(configuration(), "logger")),
+ superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
+ superstructure_(superstructure_event_loop.get()),
+ test_event_loop_(MakeEventLoop("test", roborio_)),
+ constants_fetcher_(test_event_loop_.get()),
+ simulated_robot_constants_(&constants_fetcher_.constants()),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ test_event_loop_->MakeFetcher<Output>("/superstructure")),
+ superstructure_position_fetcher_(
+ test_event_loop_->MakeFetcher<Position>("/superstructure")),
+ superstructure_position_sender_(
+ test_event_loop_->MakeSender<Position>("/superstructure")),
+ superstructure_can_position_sender_(
+ test_event_loop_->MakeSender<CANPosition>("/superstructure/rio")),
+ superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
+ superstructure_plant_(superstructure_plant_event_loop_.get(),
+ simulated_robot_constants_, dt()) {
+ set_team_id(frc971::control_loops::testing::kTeamNumber);
+
+ SetEnabled(true);
+
+ if (!absl::GetFlag(FLAGS_output_folder).empty()) {
+ unlink(absl::GetFlag(FLAGS_output_folder).c_str());
+ logger_event_loop_ = MakeEventLoop("logger", roborio_);
+ logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+ logger_->StartLoggingOnRun(absl::GetFlag(FLAGS_output_folder));
+ }
+ }
+
+ void VerifyNearGoal() {
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+ superstructure_output_fetcher_.Fetch();
+
+ ASSERT_FALSE(superstructure_status_fetcher_->estopped());
+
+ ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
+ << ": No status";
+ ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr)
+ << ": No output";
+ }
+
+ void CheckIfZeroed() {
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get()->zeroed());
+ }
+
+ void WaitUntilZeroed() {
+ int i = 0;
+ do {
+ i++;
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ // 2 Seconds
+
+ ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
+
+ // Since there is a delay when sending running, make sure we have a
+ // status before checking it.
+ } while (superstructure_status_fetcher_.get() == nullptr ||
+ !superstructure_status_fetcher_.get()->zeroed());
+ }
+
+ void WaitUntilNear() {}
+
+ const bool simulated_constants_dummy_;
+
+ const aos::Node *const roborio_;
+ const aos::Node *const logger_pi_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
+ ::y2024_bot3::control_loops::superstructure::Superstructure superstructure_;
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ const Constants *simulated_robot_constants_;
+
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<Position> superstructure_position_fetcher_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Sender<CANPosition> superstructure_can_position_sender_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
+ SuperstructureSimulation superstructure_plant_;
+
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::Logger> logger_;
+
+ const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
+};
+
+// Tests that the superstructure does nothing when the goal is to remain
+// still.
+
+TEST_F(SuperstructureTest, DoesNothing) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+ RunFor(chrono::seconds(10));
+
+ VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoal) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+ RunFor(chrono::seconds(2));
+}
+
+// Tests that running disabled works
+TEST_F(SuperstructureTest, DisableTest) {
+ RunFor(chrono::seconds(2));
+ CheckIfZeroed();
+}
+
+} // namespace y2024_bot3::control_loops::superstructure::testing
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_main.cc b/y2024_bot3/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..a7d048c
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,25 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure.h"
+
+ABSL_FLAG(std::string, arm_trajectories, "arm_trajectories_generated.bfbs",
+ "The path to the generated arm trajectories bfbs file.");
+
+using y2024_bot3::control_loops::superstructure::Superstructure;
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+
+ frc971::constants::WaitForConstants<y2024_bot3::Constants>(&config.message());
+
+ Superstructure superstructure(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_output.fbs b/y2024_bot3/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..28799de
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,6 @@
+namespace y2024_bot3.control_loops.superstructure;
+
+table Output {
+}
+
+root_type Output;
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_plotter.ts b/y2024_bot3/control_loops/superstructure/superstructure_plotter.ts
new file mode 100644
index 0000000..f3c90aa
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure_plotter.ts
@@ -0,0 +1,117 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter, MessageHandler} from '../../../aos/network/www/aos_plotter';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from '../../../aos/network/www/colors';
+import * as proxy from '../../../aos/network/www/proxy';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH * 2;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 1;
+
+// plot static zeroing single dof subsystem (generic function used by specific subsystem plotters)
+function plotSzsdofSubsystem(
+ name: string, plotter: AosPlotter, element: Element, position: MessageHandler, positionName: string,
+ status: MessageHandler, statusName: string, output: MessageHandler, outputName: string, hasPot:boolean = true): void {
+ {
+ const positionPlot =
+ plotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ positionPlot.plot.getAxisLabels().setTitle(name + ' Position');
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel('Position [rad,m]');
+ positionPlot.addMessageLine(position, [positionName, 'encoder'])
+ .setColor(RED);
+ positionPlot.addMessageLine(position, [positionName, 'absolute_encoder'])
+ .setColor(GREEN);
+ if (hasPot) {
+ positionPlot.addMessageLine(position, [positionName, 'pot'])
+ .setColor(BLUE);
+ }
+ positionPlot
+ .addMessageLine(status, [statusName, 'estimator_state', 'position'])
+ .setColor(BROWN);
+ positionPlot.addMessageLine(status, [statusName, 'position'])
+ .setColor(WHITE);
+ }
+ {
+ const statesPlot =
+ plotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ statesPlot.plot.getAxisLabels().setTitle(name + ' State');
+ statesPlot.plot.getAxisLabels().setXLabel(TIME);
+ statesPlot.plot.getAxisLabels().setYLabel('[bool,ZeroingError]');
+ statesPlot.addMessageLine(status, [statusName, 'estopped']).setColor(RED);
+ statesPlot.addMessageLine(status, [statusName, 'zeroed']).setColor(GREEN);
+ statesPlot
+ .addMessageLine(status, [statusName, 'estimator_state', 'errors[]'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+ }
+ {
+ const positionConvergencePlot =
+ plotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ positionConvergencePlot.plot.getAxisLabels().setTitle(name + ' Position Goals');
+ positionConvergencePlot.plot.getAxisLabels().setXLabel(TIME);
+ positionConvergencePlot.plot.getAxisLabels().setYLabel('[rad,m]');
+ positionConvergencePlot.addMessageLine(status, [statusName, 'position'])
+ .setColor(RED);
+ positionConvergencePlot.addMessageLine(status, [statusName, 'goal_position'])
+ .setColor(GREEN);
+ positionConvergencePlot
+ .addMessageLine(status, [statusName, 'unprofiled_goal_position'])
+ .setColor(BROWN);
+ }
+ {
+ const velocityConvergencePlot =
+ plotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ velocityConvergencePlot.plot.getAxisLabels().setTitle(name + ' Velocity Goals');
+ velocityConvergencePlot.plot.getAxisLabels().setXLabel(TIME);
+ velocityConvergencePlot.plot.getAxisLabels().setYLabel('[rad,m]');
+ velocityConvergencePlot.addMessageLine(status, [statusName, 'velocity'])
+ .setColor(RED);
+ velocityConvergencePlot.addMessageLine(status, [statusName, 'calculated_velocity'])
+ .setColor(RED).setDrawLine(false);
+ velocityConvergencePlot.addMessageLine(status, [statusName, 'goal_velocity'])
+ .setColor(GREEN);
+ velocityConvergencePlot
+ .addMessageLine(status, [statusName, 'unprofiled_goal_velocity'])
+ .setColor(BROWN);
+ }
+ {
+ const outputPlot =
+ plotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ outputPlot.plot.getAxisLabels().setTitle(name + ' Outputs');
+ outputPlot.plot.getAxisLabels().setXLabel(TIME);
+ outputPlot.plot.getAxisLabels().setYLabel('[volts]');
+ outputPlot.addMessageLine(output, [outputName])
+ .setColor(RED);
+ outputPlot.addMessageLine(status, [statusName, 'voltage_error'])
+ .setColor(GREEN);
+ outputPlot.addMessageLine(status, [statusName, 'position_power'])
+ .setColor(BLUE);
+ outputPlot.addMessageLine(status, [statusName, 'velocity_power'])
+ .setColor(BROWN);
+ outputPlot.addMessageLine(status, [statusName, 'feedforwards_power'])
+ .setColor(WHITE);
+ }
+}
+
+export function plotSuperstructure(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024_bot3.control_loops.superstructure.Status');
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+ {
+ const robotStatePlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ robotStatePlot.plot.getAxisLabels().setTitle('Robot State Plot');
+ robotStatePlot.plot.getAxisLabels().setXLabel(TIME);
+ robotStatePlot.plot.getAxisLabels().setYLabel('[bool]');
+ robotStatePlot.addMessageLine(robotState, ['outputs_enabled'])
+ .setColor(RED);
+ robotStatePlot.addMessageLine(status, ['zeroed'])
+ .setColor(GREEN);
+ robotStatePlot.addMessageLine(status, ['estopped'])
+ .setColor(BLUE);
+ }
+}
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_position.fbs b/y2024_bot3/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..f0553a5
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2024_bot3.control_loops.superstructure;
+
+table Position {
+}
+
+root_type Position;
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_replay.cc b/y2024_bot3/control_loops/superstructure/superstructure_replay.cc
new file mode 100644
index 0000000..1058f4f
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure_replay.cc
@@ -0,0 +1,75 @@
+// This binary allows us to replay the superstructure code over existing
+// logfile. When you run this code, it generates a new logfile with the data all
+// replayed, so that it can then be run through the plotting tool or analyzed
+// in some other way. The original superstructure status data will be on the
+// /original/superstructure channel.
+#include "absl/flags/flag.h"
+
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/logging/log_message_generated.h"
+#include "aos/network/team_number.h"
+#include "y2024_bot3/constants.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure.h"
+
+ABSL_FLAG(int32_t, team, 971, "Team number to use for logfile replay.");
+ABSL_FLAG(std::string, output_folder, "/tmp/superstructure_replay/",
+ "Logs all channels to the provided logfile.");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::network::OverrideTeamNumber(absl::GetFlag(FLAGS_team));
+
+ // open logfiles
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+ // TODO(james): Actually enforce not sending on the same buses as the logfile
+ // spews out.
+ reader.RemapLoggedChannel("/superstructure",
+ "y2024_bot3.control_loops.superstructure.Status");
+ reader.RemapLoggedChannel("/superstructure",
+ "y2024_bot3.control_loops.superstructure.Output");
+
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.Register(&factory);
+
+ aos::NodeEventLoopFactory *roborio =
+ factory.GetNodeEventLoopFactory("roborio");
+
+ unlink(absl::GetFlag(FLAGS_output_folder).c_str());
+ std::unique_ptr<aos::EventLoop> logger_event_loop =
+ roborio->MakeEventLoop("logger");
+ auto logger = std::make_unique<aos::logger::Logger>(logger_event_loop.get());
+ logger->StartLoggingOnRun(absl::GetFlag(FLAGS_output_folder));
+
+ roborio->OnStartup([roborio]() {
+ roborio->AlwaysStart<
+ y2024_bot3::control_loops::superstructure::Superstructure>(
+ "superstructure");
+ });
+
+ std::unique_ptr<aos::EventLoop> print_loop = roborio->MakeEventLoop("print");
+ print_loop->SkipAosLog();
+ print_loop->MakeWatcher(
+ "/aos", [&print_loop](const aos::logging::LogMessageFbs &msg) {
+ LOG(INFO) << print_loop->context().monotonic_event_time << " "
+ << aos::FlatbufferToJson(&msg);
+ });
+ print_loop->MakeWatcher(
+ "/superstructure",
+ [&](const y2024_bot3::control_loops::superstructure::Status &status) {
+ if (status.estopped()) {
+ LOG(ERROR) << "Estopped";
+ }
+ });
+
+ factory.Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_status.fbs b/y2024_bot3/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..ca5876e
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,14 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2024_bot3.control_loops.superstructure;
+
+table Status {
+ // All subsystems know their location.
+ zeroed:bool (id: 0);
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ estopped:bool (id: 1);
+}
+
+root_type Status;
diff --git a/y2024_bot3/copy_logs.sh b/y2024_bot3/copy_logs.sh
new file mode 100755
index 0000000..806460f
--- /dev/null
+++ b/y2024_bot3/copy_logs.sh
@@ -0,0 +1,29 @@
+#!/bin/bash
+
+# Helper script to copy most recent logs off of the pis
+
+set -e
+
+ROBOT_PREFIX="99" # ..71 (Should be one of 79, 89, 99, or 9)
+PI_LIST="1 2" # Should be some set of {1,2,3,4,5,6}
+
+LOG_FILE_PATH=/media/sda1/fbs_log-current
+if [[ -z $1 || ! -d $1 ]]; then
+ echo "Please specify the base directory to store the logs ('$1' not found)"
+ exit -1
+fi
+
+# Create output directory based on given directory + a timestamp
+OUTPUT_DIR=$1"/"`date +"%Y-%m-%dT%H-%M-%S"`
+mkdir ${OUTPUT_DIR}
+
+echo "Copying logs from the robot ${ROBOT_PREFIX}71 and pis ${PI_LIST}"
+echo "Storing logs in folder ${OUTPUT_DIR}"
+
+for pi in $PI_LIST; do
+ echo "========================================================"
+ echo "Copying logs from pi-${ROBOT_PREFIX}71-$pi"
+ echo "========================================================"
+ scp -r pi@10.${ROBOT_PREFIX}.71.10${pi}:${LOG_FILE_PATH} ${OUTPUT_DIR}/fbs_log-pi${pi}
+done
+
diff --git a/y2024_bot3/joystick_reader.cc b/y2024_bot3/joystick_reader.cc
new file mode 100644
index 0000000..4b88402
--- /dev/null
+++ b/y2024_bot3/joystick_reader.cc
@@ -0,0 +1,63 @@
+#include <unistd.h>
+
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+
+#include "absl/flags/flag.h"
+
+#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/network/team_number.h"
+#include "aos/util/log_interval.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/input/action_joystick_input.h"
+#include "frc971/input/driver_station_data.h"
+#include "frc971/input/drivetrain_input.h"
+#include "frc971/input/joystick_input.h"
+#include "frc971/input/redundant_joystick_data.h"
+#include "frc971/zeroing/wrap.h"
+#include "y2024_bot3/constants/constants_generated.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_goal_static.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_status_static.h"
+
+using frc971::CreateProfileParameters;
+using frc971::input::driver_station::ButtonLocation;
+using frc971::input::driver_station::ControlBit;
+using frc971::input::driver_station::JoystickAxis;
+using frc971::input::driver_station::POVLocation;
+
+namespace y2024_bot3::input::joysticks {
+
+namespace superstructure = y2024_bot3::control_loops::superstructure;
+
+// ButtonLocation constants go here
+
+class Reader : public ::frc971::input::ActionJoystickInput {};
+} // namespace y2024_bot3::input::joysticks
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+ frc971::constants::WaitForConstants<y2024_bot3::Constants>(&config.message());
+
+ ::aos::ShmEventLoop constant_fetcher_event_loop(&config.message());
+ frc971::constants::ConstantsFetcher<y2024_bot3::Constants> constants_fetcher(
+ &constant_fetcher_event_loop);
+ const y2024_bot3::Constants *robot_constants = &constants_fetcher.constants();
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ (void)robot_constants;
+ //::y2024_bot3::input::joysticks::Reader reader(&event_loop, robot_constants);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024_bot3/log_web_proxy.sh b/y2024_bot3/log_web_proxy.sh
new file mode 100755
index 0000000..f97bafe
--- /dev/null
+++ b/y2024_bot3/log_web_proxy.sh
@@ -0,0 +1 @@
+./aos/network/log_web_proxy_main --data_dir=y2024_bot3/www $@
diff --git a/y2024_bot3/orin/BUILD b/y2024_bot3/orin/BUILD
new file mode 100644
index 0000000..c8b899b
--- /dev/null
+++ b/y2024_bot3/orin/BUILD
@@ -0,0 +1,16 @@
+cc_binary(
+ name = "can_logger",
+ srcs = [
+ "can_logger_main.cc",
+ ],
+ target_compatible_with = ["@platforms//cpu:arm64"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/time",
+ "//frc971/can_logger:can_logger_lib",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
+ ],
+)
diff --git a/y2024_bot3/orin/can_logger_main.cc b/y2024_bot3/orin/can_logger_main.cc
new file mode 100644
index 0000000..dcfec69
--- /dev/null
+++ b/y2024_bot3/orin/can_logger_main.cc
@@ -0,0 +1,18 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/can_logger/can_logger.h"
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+
+ frc971::can_logger::CanLogger cana_logger(&event_loop, "/can/cana", "cana");
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024_bot3/vision/BUILD b/y2024_bot3/vision/BUILD
new file mode 100644
index 0000000..b55d35d
--- /dev/null
+++ b/y2024_bot3/vision/BUILD
@@ -0,0 +1,54 @@
+filegroup(
+ name = "image_streamer_start",
+ srcs = ["image_streamer_start.sh"],
+ visibility = ["//visibility:public"],
+)
+
+cc_binary(
+ name = "apriltag_detector",
+ srcs = [
+ "apriltag_detector.cc",
+ "vision_util.cc",
+ "vision_util.h",
+ ],
+ features = ["cuda"],
+ target_compatible_with = ["@platforms//cpu:arm64"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/orin:gpu_apriltag_lib",
+ "//third_party:cudart",
+ "//third_party/apriltag",
+ "//y2024_bot3/constants:constants_fbs",
+ "@com_github_nvidia_cccl//:cccl",
+ "@com_google_absl//absl/flags:flag",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
+ ],
+)
+
+cc_binary(
+ name = "viewer",
+ srcs = [
+ "viewer.cc",
+ "vision_util.cc",
+ "vision_util.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = [
+ "//y2024_bot3:__subpackages__",
+ ],
+ deps = [
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/vision:vision_fbs",
+ "//frc971/vision:vision_util_lib",
+ "//third_party:opencv",
+ "//y2024_bot3/constants:constants_fbs",
+ "@com_google_absl//absl/strings",
+ ],
+)
diff --git a/y2024_bot3/vision/apriltag_detector.cc b/y2024_bot3/vision/apriltag_detector.cc
new file mode 100644
index 0000000..2c73c58
--- /dev/null
+++ b/y2024_bot3/vision/apriltag_detector.cc
@@ -0,0 +1,54 @@
+
+#include <string>
+
+#include "absl/flags/flag.h"
+
+#include "aos/init.h"
+#include "frc971/orin/gpu_apriltag.h"
+#include "y2024_bot3/constants/constants_generated.h"
+#include "y2024_bot3/vision/vision_util.h"
+
+ABSL_FLAG(std::string, channel, "/camera", "Channel name");
+ABSL_FLAG(std::string, config, "aos_config.json",
+ "Path to the config file to use.");
+
+void GpuApriltagDetector() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(absl::GetFlag(FLAGS_config));
+
+ frc971::constants::WaitForConstants<y2024_bot3::Constants>(&config.message());
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ const frc971::constants::ConstantsFetcher<y2024_bot3::Constants>
+ calibration_data(&event_loop);
+
+ CHECK(absl::GetFlag(FLAGS_channel).length() == 8);
+ int camera_id = std::stoi(absl::GetFlag(FLAGS_channel).substr(7, 1));
+ const frc971::vision::calibration::CameraCalibration *calibration =
+ y2024_bot3::vision::FindCameraCalibration(
+ calibration_data.constants(),
+ event_loop.node()->name()->string_view(), camera_id);
+
+ frc971::apriltag::ApriltagDetector detector(
+ &event_loop, absl::GetFlag(FLAGS_channel), calibration);
+
+ // TODO(austin): Figure out our core pinning strategy.
+ // event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({5}));
+
+ LOG(INFO) << "Setting scheduler priority";
+ struct sched_param param;
+ param.sched_priority = 21;
+ PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0);
+
+ LOG(INFO) << "Running event loop";
+ // TODO(austin): Pre-warm it...
+ event_loop.Run();
+} // namespace frc971::apriltag
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ GpuApriltagDetector();
+
+ return 0;
+}
diff --git a/y2024_bot3/vision/image_streamer_start.sh b/y2024_bot3/vision/image_streamer_start.sh
new file mode 100755
index 0000000..48d9da7
--- /dev/null
+++ b/y2024_bot3/vision/image_streamer_start.sh
@@ -0,0 +1,22 @@
+#!/bin/bash
+
+# Some configurations to avoid dropping frames
+# 640x480@30fps, 400x300@60fps.
+# Bitrate 500000-1500000
+WIDTH=640
+HEIGHT=480
+BITRATE=1500000
+LISTEN_ON="/camera/downsized"
+# Don't interfere with field webpage
+STREAMING_PORT=1181
+
+# Handle weirdness with openssl and gstreamer
+export OPENSSL_CONF=""
+
+# Enable for verbose logging
+#export GST_DEBUG=4
+
+export LD_LIBRARY_PATH=/usr/lib/aarch64-linux-gnu/gstreamer-1.0
+
+exec ./image_streamer --width=$WIDTH --height=$HEIGHT --bitrate=$BITRATE --listen_on=$LISTEN_ON --config=aos_config.json --streaming_port=$STREAMING_PORT
+
diff --git a/y2024_bot3/vision/maps/BUILD b/y2024_bot3/vision/maps/BUILD
new file mode 100644
index 0000000..38191a4
--- /dev/null
+++ b/y2024_bot3/vision/maps/BUILD
@@ -0,0 +1,7 @@
+filegroup(
+ name = "maps",
+ srcs = glob([
+ "*.json",
+ ]),
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024_bot3/vision/maps/target_map.json b/y2024_bot3/vision/maps/target_map.json
new file mode 100644
index 0000000..2a8dfef
--- /dev/null
+++ b/y2024_bot3/vision/maps/target_map.json
@@ -0,0 +1,236 @@
+{
+/* Targets have positive Z axis pointing into the board, positive X to the right
+ when looking at the board, and positive Y is down when looking at the board.
+ This means that you will get an identity rotation from the camera to target
+ frame when the target is upright, flat, and centered in the camera's view.
+
+ The global frame as the origin at the center of the field, positive X points
+ at the red driver's station, and positive Z points straight up.
+ */
+ "target_poses": [
+ {
+ "id": 1,
+ "position": {
+ "x": 6.809,
+ "y": -3.860,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": 0.6830127,
+ "z": -0.6830127
+ }
+ },
+ {
+ "id": 2,
+ "position": {
+ "x": 7.915,
+ "y": -3.223,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": 0.6830127,
+ "z": -0.6830127
+ }
+ },
+ {
+ "id": 3,
+ "position": {
+ "x": 8.309,
+ "y": 0.877,
+ "z": 1.456
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": 0.5,
+ "z": -0.5
+ }
+ },
+ {
+ "id": 4,
+ "position": {
+ "x": 8.309,
+ "y": 1.442,
+ "z": 1.456
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": 0.5,
+ "z": -0.5
+ }
+ },
+ {
+ "id": 5,
+ "position": {
+ "x": 6.428,
+ "y": 4.099,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.7071068,
+ "x": -0.7071068,
+ "y": 0.0,
+ "z": 0.0
+ }
+ },
+ {
+ "id": 6,
+ "position": {
+ "x": -6.430,
+ "y": 4.099,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.7071068,
+ "x": -0.7071068,
+ "y": 0.0,
+ "z": 0.0
+ }
+ },
+ {
+ "id": 7,
+ "position": {
+ "x": -8.309,
+ "y": 1.442,
+ "z": 1.474
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
+ }
+ },
+ {
+ "id": 8,
+ "position": {
+ "x": -8.309,
+ "y": 0.877,
+ "z": 1.474
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
+ }
+ },
+ {
+ "id": 9,
+ "position": {
+ "x": -7.915,
+ "y": -3.223,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": -0.6830127,
+ "z": 0.6830127
+ }
+ },
+ {
+ "id": 10,
+ "position": {
+ "x": -6.809,
+ "y": -3.860,
+ "z": 1.361
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": -0.6830127,
+ "z": 0.6830127
+ }
+ },
+ {
+ "id": 11,
+ "position": {
+ "x": 3.629,
+ "y": -0.393,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.6830127,
+ "x": -0.6830127,
+ "y": -0.1830127,
+ "z": 0.1830127
+ }
+ },
+ {
+ "id": 12,
+ "position": {
+ "x": 3.630,
+ "y": 0.392,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": -0.6830127,
+ "z": 0.6830127
+ }
+ },
+ {
+ "id": 13,
+ "position": {
+ "x": 2.949,
+ "y": -0.000,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": 0.5,
+ "z": -0.5
+ }
+ },
+ {
+ "id": 14,
+ "position": {
+ "x": -2.949,
+ "y": -0.000,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.5,
+ "x": -0.5,
+ "y": -0.5,
+ "z": 0.5
+ }
+ },
+ {
+ "id": 15,
+ "position": {
+ "x": -3.629,
+ "y": 0.393,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": 0.6830127,
+ "z": -0.6830127
+ }
+ },
+ {
+ "id": 16,
+ "position": {
+ "x": -3.630,
+ "y": -0.392,
+ "z": 1.326
+ },
+ "orientation": {
+ "w": 0.6830127,
+ "x": -0.6830127,
+ "y": 0.1830127,
+ "z": -0.1830127
+ }
+ }
+ ]
+}
diff --git a/y2024_bot3/vision/viewer.cc b/y2024_bot3/vision/viewer.cc
new file mode 100644
index 0000000..2d99fd5
--- /dev/null
+++ b/y2024_bot3/vision/viewer.cc
@@ -0,0 +1,122 @@
+#include "absl/strings/match.h"
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/time/time.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/vision/vision_generated.h"
+#include "frc971/vision/vision_util_lib.h"
+#include "y2024_bot3/vision/vision_util.h"
+
+ABSL_FLAG(std::string, capture, "",
+ "If set, capture a single image and save it to this filename.");
+ABSL_FLAG(std::string, channel, "/camera", "Channel name for the image.");
+ABSL_FLAG(std::string, config, "aos_config.json",
+ "Path to the config file to use.");
+ABSL_FLAG(int32_t, rate, 100, "Time in milliseconds to wait between images");
+ABSL_FLAG(double, scale, 1.0, "Scale factor for images being displayed");
+
+namespace y2024_bot3::vision {
+namespace {
+
+using frc971::vision::CameraImage;
+
+bool DisplayLoop(const cv::Mat intrinsics, const cv::Mat dist_coeffs,
+ aos::Fetcher<CameraImage> *image_fetcher) {
+ const CameraImage *image;
+
+ // Read next image
+ if (!image_fetcher->Fetch()) {
+ VLOG(2) << "Couldn't fetch next image";
+ return true;
+ }
+ image = image_fetcher->get();
+ CHECK(image != nullptr) << "Couldn't read image";
+
+ // Create color image:
+ cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
+ (void *)image->data()->data());
+ cv::Mat bgr_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
+
+ if (!absl::GetFlag(FLAGS_capture).empty()) {
+ if (absl::EndsWith(absl::GetFlag(FLAGS_capture), ".bfbs")) {
+ aos::WriteFlatbufferToFile(absl::GetFlag(FLAGS_capture),
+ image_fetcher->CopyFlatBuffer());
+ } else {
+ cv::imwrite(absl::GetFlag(FLAGS_capture), bgr_image);
+ }
+
+ return false;
+ }
+
+ cv::Mat undistorted_image;
+ cv::undistort(bgr_image, undistorted_image, intrinsics, dist_coeffs);
+ if (absl::GetFlag(FLAGS_scale) != 1.0) {
+ cv::resize(undistorted_image, undistorted_image, cv::Size(),
+ absl::GetFlag(FLAGS_scale), absl::GetFlag(FLAGS_scale));
+ }
+ cv::imshow("Display", undistorted_image);
+
+ int keystroke = cv::waitKey(1);
+ if ((keystroke & 0xFF) == static_cast<int>('c')) {
+ // Convert again, to get clean image
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
+ std::stringstream name;
+ name << "capture-" << aos::realtime_clock::now() << ".png";
+ cv::imwrite(name.str(), bgr_image);
+ LOG(INFO) << "Saved image file: " << name.str();
+ } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ return false;
+ }
+ return true;
+}
+
+void ViewerMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(absl::GetFlag(FLAGS_config));
+
+ frc971::constants::WaitForConstants<y2024_bot3::Constants>(&config.message());
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ frc971::constants::ConstantsFetcher<y2024_bot3::Constants> constants_fetcher(
+ &event_loop);
+ CHECK(absl::GetFlag(FLAGS_channel).length() == 8);
+ int camera_id = std::stoi(absl::GetFlag(FLAGS_channel).substr(7, 1));
+ const auto *calibration_data = FindCameraCalibration(
+ constants_fetcher.constants(), event_loop.node()->name()->string_view(),
+ camera_id);
+ const cv::Mat intrinsics = frc971::vision::CameraIntrinsics(calibration_data);
+ const cv::Mat dist_coeffs =
+ frc971::vision::CameraDistCoeffs(calibration_data);
+
+ aos::Fetcher<CameraImage> image_fetcher =
+ event_loop.MakeFetcher<CameraImage>(absl::GetFlag(FLAGS_channel));
+
+ // Run the display loop
+ event_loop.AddPhasedLoop(
+ [&](int) {
+ if (!DisplayLoop(intrinsics, dist_coeffs, &image_fetcher)) {
+ LOG(INFO) << "Calling event_loop Exit";
+ event_loop.Exit();
+ };
+ },
+ ::std::chrono::milliseconds(absl::GetFlag(FLAGS_rate)));
+
+ event_loop.Run();
+
+ image_fetcher = aos::Fetcher<CameraImage>();
+}
+
+} // namespace
+} // namespace y2024_bot3::vision
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2024_bot3::vision::ViewerMain();
+}
diff --git a/y2024_bot3/vision/vision_util.cc b/y2024_bot3/vision/vision_util.cc
new file mode 100644
index 0000000..0f99849
--- /dev/null
+++ b/y2024_bot3/vision/vision_util.cc
@@ -0,0 +1,49 @@
+#include "y2024_bot3/vision/vision_util.h"
+
+#include "absl/log/check.h"
+#include "absl/log/log.h"
+
+namespace y2024_bot3::vision {
+
+// Store a list of ordered cameras as you progress around the robot/box of orins
+std::vector<CameraNode> CreateNodeList() {
+ std::vector<CameraNode> list;
+
+ list.push_back({.node_name = "imu", .camera_number = 0});
+ list.push_back({.node_name = "imu", .camera_number = 1});
+ list.push_back({.node_name = "orin1", .camera_number = 1});
+ list.push_back({.node_name = "orin1", .camera_number = 0});
+
+ return list;
+}
+
+// From the node_list, create a numbering scheme from 0 to 3
+std::map<std::string, int> CreateOrderingMap(
+ std::vector<CameraNode> &node_list) {
+ std::map<std::string, int> map;
+
+ for (uint i = 0; i < node_list.size(); i++) {
+ map.insert({node_list.at(i).camera_name(), i});
+ }
+
+ return map;
+}
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2024_bot3::Constants &calibration_data, std::string_view node_name,
+ int camera_number) {
+ CHECK(calibration_data.has_cameras());
+ for (const y2024_bot3::CameraConfiguration *candidate :
+ *calibration_data.cameras()) {
+ CHECK(candidate->has_calibration());
+ if (candidate->calibration()->node_name()->string_view() != node_name ||
+ candidate->calibration()->camera_number() != camera_number) {
+ continue;
+ }
+ return candidate->calibration();
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+ << " and camera number " << camera_number;
+}
+
+} // namespace y2024_bot3::vision
diff --git a/y2024_bot3/vision/vision_util.h b/y2024_bot3/vision/vision_util.h
new file mode 100644
index 0000000..fc0f29c
--- /dev/null
+++ b/y2024_bot3/vision/vision_util.h
@@ -0,0 +1,41 @@
+#ifndef Y2024_BOT3_VISION_VISION_UTIL_H_
+#define Y2024_BOT3_VISION_VISION_UTIL_H_
+#include <map>
+#include <string_view>
+
+#include "opencv2/imgproc.hpp"
+
+#include "y2024_bot3/constants/constants_generated.h"
+
+namespace y2024_bot3::vision {
+
+// Generate unique colors for each camera
+const auto kOrinColors = std::map<std::string, cv::Scalar>{
+ {"/orin1/camera0", cv::Scalar(255, 0, 255)},
+ {"/orin1/camera1", cv::Scalar(255, 255, 0)},
+ {"/imu/camera0", cv::Scalar(0, 255, 255)},
+ {"/imu/camera1", cv::Scalar(255, 165, 0)},
+};
+
+// Structure to store node name (e.g., orin1, imu), number, and a usable string
+struct CameraNode {
+ std::string node_name;
+ int camera_number;
+
+ inline const std::string camera_name() const {
+ return "/" + node_name + "/camera" + std::to_string(camera_number);
+ }
+};
+
+std::vector<CameraNode> CreateNodeList();
+
+std::map<std::string, int> CreateOrderingMap(
+ std::vector<CameraNode> &node_list);
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2024_bot3::Constants &calibration_data, std::string_view node_name,
+ int camera_number);
+
+} // namespace y2024_bot3::vision
+
+#endif // Y2024_BOT3_VISION_VISION_UTIL_H_
diff --git a/y2024_bot3/wpilib_interface.cc b/y2024_bot3/wpilib_interface.cc
new file mode 100644
index 0000000..b6066e3
--- /dev/null
+++ b/y2024_bot3/wpilib_interface.cc
@@ -0,0 +1,249 @@
+#include <unistd.h>
+
+#include <array>
+#include <chrono>
+#include <cinttypes>
+#include <cstdio>
+#include <cstring>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#include "absl/flags/flag.h"
+
+#include "frc971/wpilib/ahal/AnalogInput.h"
+#include "frc971/wpilib/ahal/DriverStation.h"
+#include "frc971/wpilib/ahal/Encoder.h"
+#include "frc971/wpilib/ahal/TalonFX.h"
+#include "frc971/wpilib/ahal/VictorSP.h"
+#undef ERROR
+
+#include "ctre/phoenix/cci/Diagnostics_CCI.h"
+
+#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"
+#include "aos/logging/logging.h"
+#include "aos/realtime.h"
+#include "aos/time/time.h"
+#include "aos/util/log_interval.h"
+#include "aos/util/phased_loop.h"
+#include "aos/util/wrapping_counter.h"
+#include "frc971/can_configuration_generated.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/input/robot_state_generated.h"
+#include "frc971/queues/gyro_generated.h"
+#include "frc971/wpilib/buffered_pcm.h"
+#include "frc971/wpilib/buffered_solenoid.h"
+#include "frc971/wpilib/can_sensor_reader.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/generic_can_writer.h"
+#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/logging_generated.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/talonfx.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+#include "y2024_bot3/constants.h"
+#include "y2024_bot3/constants/constants_generated.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_can_position_static.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2024_bot3/control_loops/superstructure/superstructure_position_static.h"
+
+ABSL_FLAG(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 ::frc971::CANConfiguration;
+using ::frc971::wpilib::TalonFX;
+using ::y2024_bot3::constants::Values;
+namespace superstructure = ::y2024_bot3::control_loops::superstructure;
+namespace chrono = ::std::chrono;
+using std::make_unique;
+
+namespace y2024_bot3::wpilib {
+namespace {
+
+constexpr double kMaxBringupPower = 12.0;
+
+constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
+ 1000000 // arbitrary number because we deleted all the stuff in this array
+});
+
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+ "fast encoders are too fast");
+
+} // namespace
+
+// Class to send position messages with sensor readings to our loops.
+class SensorReader : public ::frc971::wpilib::SensorReader {
+ public:
+ SensorReader(::aos::ShmEventLoop *event_loop,
+ const Constants *robot_constants)
+ : ::frc971::wpilib::SensorReader(event_loop),
+ robot_constants_(robot_constants),
+ superstructure_position_sender_(
+ event_loop->MakeSender<superstructure::PositionStatic>(
+ "/superstructure")) {
+ UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
+ event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+ };
+ void Start() override { AddToDMA(&imu_yaw_rate_reader_); }
+
+ 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 {
+ aos::Sender<superstructure::PositionStatic>::StaticBuilder builder =
+ superstructure_position_sender_.MakeStaticBuilder();
+
+ builder.CheckOk(builder.Send());
+
+ {
+ auto builder = gyro_sender_.MakeBuilder();
+ ::frc971::sensors::GyroReading::Builder gyro_reading_builder =
+ builder.MakeBuilder<::frc971::sensors::GyroReading>();
+
+ builder.CheckOk(builder.Send(gyro_reading_builder.Finish()));
+ }
+ }
+
+ private:
+ const Constants *robot_constants_;
+
+ aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
+ ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
+
+ std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
+
+ frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
+};
+
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
+ public:
+ ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
+ return make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
+ frc::Encoder::k4X);
+ }
+
+ void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ frc971::constants::WaitForConstants<y2024_bot3::Constants>(
+ &config.message());
+
+ ::aos::ShmEventLoop constant_fetcher_event_loop(&config.message());
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher(
+ &constant_fetcher_event_loop);
+ const Constants *robot_constants = &constants_fetcher.constants();
+
+ AddLoop(&constant_fetcher_event_loop);
+
+ // 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);
+ AddLoop(&pdp_fetcher_event_loop);
+
+ // Thread 3.
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
+ SensorReader sensor_reader(&sensor_reader_event_loop, robot_constants);
+ sensor_reader.set_pwm_trigger(false);
+ sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(25));
+
+ AddLoop(&sensor_reader_event_loop);
+
+ // Thread 4.
+ // Set up CAN.
+ if (!absl::GetFlag(FLAGS_ctre_diag_server)) {
+ c_Phoenix_Diagnostics_SetSecondsToStart(-1);
+ c_Phoenix_Diagnostics_Dispose();
+ }
+
+ std::vector<ctre::phoenix6::BaseStatusSignal *> canivore_signal_registry;
+ std::vector<ctre::phoenix6::BaseStatusSignal *> rio_signal_registry;
+
+ ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+ can_sensor_reader_event_loop.set_name("CANSensorReader");
+
+ ::aos::ShmEventLoop rio_sensor_reader_event_loop(&config.message());
+ rio_sensor_reader_event_loop.set_name("RioSensorReader");
+
+ // Creating list of talonfx for CANSensorReader
+ std::vector<std::shared_ptr<TalonFX>> canivore_talonfxs;
+ std::vector<std::shared_ptr<TalonFX>> rio_talonfxs;
+
+ aos::Sender<y2024_bot3::control_loops::superstructure::CANPositionStatic>
+ superstructure_can_position_sender =
+ can_sensor_reader_event_loop.MakeSender<
+ y2024_bot3::control_loops::superstructure::CANPositionStatic>(
+ "/superstructure/canivore");
+
+ aos::Sender<y2024_bot3::control_loops::superstructure::CANPositionStatic>
+ superstructure_rio_position_sender =
+ rio_sensor_reader_event_loop.MakeSender<
+ y2024_bot3::control_loops::superstructure::CANPositionStatic>(
+ "/superstructure/rio");
+
+ frc971::wpilib::CANSensorReader rio_can_sensor_reader(
+ &rio_sensor_reader_event_loop, std::move(rio_signal_registry),
+ rio_talonfxs,
+ [&superstructure_rio_position_sender](
+ ctre::phoenix::StatusCode status) {
+ aos::Sender<
+ y2024_bot3::control_loops::superstructure::CANPositionStatic>::
+ StaticBuilder superstructure_can_builder =
+ superstructure_rio_position_sender.MakeStaticBuilder();
+
+ superstructure_can_builder->set_status(static_cast<int>(status));
+ superstructure_can_builder.CheckOk(superstructure_can_builder.Send());
+ },
+ frc971::wpilib::CANSensorReader::SignalSync::kNoSync);
+
+ AddLoop(&can_sensor_reader_event_loop);
+ AddLoop(&rio_sensor_reader_event_loop);
+
+ // Thread 5.
+ ::aos::ShmEventLoop can_output_event_loop(&config.message());
+ can_output_event_loop.set_name("CANOutputWriter");
+
+ frc971::wpilib::GenericCANWriter<control_loops::superstructure::Output>
+ can_superstructure_writer(
+ &can_output_event_loop,
+ [](const control_loops::superstructure::Output &output,
+ const std::map<std::string_view, std::shared_ptr<TalonFX>>
+ &talonfx_map) {
+ (void)output;
+ (void)talonfx_map;
+ });
+
+ can_output_event_loop.MakeWatcher(
+ "/roborio", [&can_superstructure_writer](
+ const frc971::CANConfiguration &configuration) {
+ can_superstructure_writer.HandleCANConfiguration(configuration);
+ });
+
+ AddLoop(&can_output_event_loop);
+
+ RunLoops();
+ }
+};
+
+} // namespace y2024_bot3::wpilib
+
+AOS_ROBOT_CLASS(::y2024_bot3::wpilib::WPILibRobot);
diff --git a/y2024_bot3/www/BUILD b/y2024_bot3/www/BUILD
new file mode 100644
index 0000000..51d7c8f
--- /dev/null
+++ b/y2024_bot3/www/BUILD
@@ -0,0 +1,68 @@
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
+
+filegroup(
+ name = "files",
+ srcs = glob([
+ "**/*.html",
+ "**/*.css",
+ "**/*.png",
+ ]) + ["2024_bot3.png"],
+ visibility = ["//visibility:public"],
+)
+
+genrule(
+ name = "2024_bot3_field_png",
+ srcs = ["//third_party/y2024/field:pictures"],
+ outs = ["2024_bot3.png"],
+ cmd = "cp third_party/y2024/field/2024.png $@",
+)
+
+ts_project(
+ name = "field_main",
+ srcs = [
+ "constants.ts",
+ "field_handler.ts",
+ "field_main.ts",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network:connect_ts_fbs",
+ "//aos/network:message_bridge_client_ts_fbs",
+ "//aos/network:message_bridge_server_ts_fbs",
+ "//aos/network:web_proxy_ts_fbs",
+ "//aos/network/www:proxy",
+ "//frc971/control_loops:control_loops_ts_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_ts_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_ts_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
+ "//frc971/vision:target_map_ts_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_position_ts_fbs",
+ "//y2024_bot3/control_loops/superstructure:superstructure_status_ts_fbs",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ ],
+)
+
+rollup_bundle(
+ name = "field_main_bundle",
+ entry_point = "field_main.ts",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2024_bot3:__subpackages__"],
+ deps = [
+ ":field_main",
+ ],
+)
+
+aos_downloader_dir(
+ name = "www_files",
+ srcs = [
+ ":field_main_bundle.min.js",
+ ":files",
+ "//frc971/analysis:plot_index_bundle.min.js",
+ "//frc971/www:starter_files",
+ ],
+ dir = "www",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024_bot3/www/constants.ts b/y2024_bot3/www/constants.ts
new file mode 100644
index 0000000..97d6e28
--- /dev/null
+++ b/y2024_bot3/www/constants.ts
@@ -0,0 +1,8 @@
+// Conversion constants to meters
+export const IN_TO_M = 0.0254;
+export const FT_TO_M = 0.3048;
+// Dimensions of the field in meters
+// Numbers are slightly hand-tuned to match the PNG that we are using.
+export const FIELD_WIDTH = 26 * FT_TO_M + 11.25 * IN_TO_M;
+export const FIELD_LENGTH = 54 * FT_TO_M + 3.25 * IN_TO_M;
+
diff --git a/y2024_bot3/www/field.html b/y2024_bot3/www/field.html
new file mode 100644
index 0000000..04fb5c1
--- /dev/null
+++ b/y2024_bot3/www/field.html
@@ -0,0 +1,101 @@
+<html>
+
+<head>
+ <script src="field_main_bundle.min.js" defer></script>
+ <link rel="stylesheet" href="styles.css">
+</head>
+
+<body>
+ <div style="display: grid;
+ grid-template-columns: auto auto auto; gap: 5px;">
+ <div>
+ <div id="field"> </div>
+ <div id="legend"> </div>
+ <h3>Zeroing Faults:</h3>
+ <p id="zeroing_faults"> NA </p>
+ </div>
+ <div>
+ <table>
+ <tr>
+ <th colspan="2" style="text-align: center;">Robot State</th>
+ </tr>
+ <tr>
+ <td>X</td>
+ <td id="x"> NA </td>
+ </tr>
+ <tr>
+ <td>Y</td>
+ <td id="y"> NA </td>
+ </tr>
+ <tr>
+ <td>Theta</td>
+ <td id="theta"> NA </td>
+ </tr>
+ </table>
+ <table>
+ <tr>
+ <th colspan="2" style="text-align: center;">Images</th>
+ </tr>
+ <tr>
+ <td> Images Accepted </td>
+ <td id="images_accepted"> NA </td>
+ </tr>
+ </table>
+ <table>
+ <tr>
+ <th colspan="2" style="text-align: center;">Superstructure States</th>
+ </tr>
+ <tr>
+ <td style="font-weight: bold;">Superstructure State</td>
+ <td id="superstructure_state" style="font-weight: bold;"> NA </td>
+ </tr>
+ </table>
+ </div>
+ <div>
+ <table>
+ <tr>
+ <th colspan="2" style="text-align: center;">Subsystems</th>
+ </tr>
+ </table>
+ <table>
+ <tr>
+ <th colspan="2" style="text-align: center;"> Drivetrain Encoder Positions </th>
+ </tr>
+ <tr>
+ <td> Left Encoder Position</td>
+ <td id="left_drivetrain_encoder"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Encoder Position</td>
+ <td id="right_drivetrain_encoder"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Front Falcon CAN Position</td>
+ <td id="falcon_right_front"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Back Falcon CAN Position</td>
+ <td id="falcon_right_back"> NA </td>
+ </tr>
+ <tr>
+ <td> Left Front Falcon CAN Position</td>
+ <td id="falcon_left_front"> NA </td>
+ </tr>
+ <tr>
+ <td> Left Back Falcon CAN Position</td>
+ <td id="falcon_left_back"> NA </td>
+ </tr>
+ </table>
+ </div>
+ </div>
+ <div id="message_bridge_status">
+ <div>
+ <div>Node</div>
+ <div>Client</div>
+ <div>Server</div>
+ </div>
+ </div>
+ <div id="vision_readouts"> </div>
+</body>
+
+</html>
\ No newline at end of file
diff --git a/y2024_bot3/www/field_handler.ts b/y2024_bot3/www/field_handler.ts
new file mode 100644
index 0000000..5b243ef
--- /dev/null
+++ b/y2024_bot3/www/field_handler.ts
@@ -0,0 +1,347 @@
+import {ByteBuffer} from 'flatbuffers'
+import {ClientStatistics} from '../../aos/network/message_bridge_client_generated'
+import {ServerStatistics, State as ConnectionState} from '../../aos/network/message_bridge_server_generated'
+import {Connection} from '../../aos/network/www/proxy'
+import {ZeroingError} from '../../frc971/control_loops/control_loops_generated'
+import {Position as DrivetrainPosition} from '../../frc971/control_loops/drivetrain/drivetrain_position_generated'
+import {CANPosition as DrivetrainCANPosition} from '../../frc971/control_loops/drivetrain/drivetrain_can_position_generated'
+import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated'
+import {Position as SuperstructurePosition} from '../control_loops/superstructure/superstructure_position_generated'
+import {Status as SuperstructureStatus} from '../control_loops/superstructure/superstructure_status_generated'
+import {TargetMap} from '../../frc971/vision/target_map_generated'
+
+
+import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+
+// (0,0) is field center, +X is toward red DS
+const FIELD_SIDE_Y = FIELD_WIDTH / 2;
+const FIELD_EDGE_X = FIELD_LENGTH / 2;
+
+const ROBOT_WIDTH = 29 * IN_TO_M;
+const ROBOT_LENGTH = 32 * IN_TO_M;
+
+const CAMERA_COLORS = ['#ff00ff', '#ffff00', '#00ffff', '#ffa500'];
+const CAMERAS = ['/orin1/camera0', '/orin1/camera1', '/imu/camera0', '/imu/camera1'];
+
+export class FieldHandler {
+ private canvas = document.createElement('canvas');
+ private superstructureStatus: SuperstructureStatus|null = null;
+ private superstructurePosition: SuperstructurePosition|null = null;
+
+ // Image information indexed by timestamp (seconds since the epoch), so that
+ // we can stop displaying images after a certain amount of time.
+ private x: HTMLElement = (document.getElementById('x') as HTMLElement);
+ private y: HTMLElement = (document.getElementById('y') as HTMLElement);
+ private theta: HTMLElement =
+ (document.getElementById('theta') as HTMLElement);
+
+ private imagesAcceptedCounter: HTMLElement =
+ (document.getElementById('images_accepted') as HTMLElement);
+ // HTML elements for rejection reasons for individual cameras. Indices
+ // corresponding to RejectionReason enum values will be for those reasons. The
+ // final row will account for images rejected by the aprilrobotics detector
+ // instead of the localizer.
+ private rejectionReasonCells: HTMLElement[][] = [];
+ private messageBridgeDiv: HTMLElement =
+ (document.getElementById('message_bridge_status') as HTMLElement);
+ private clientStatuses = new Map<string, HTMLElement>();
+ private serverStatuses = new Map<string, HTMLElement>();
+
+ private fieldImage: HTMLImageElement = new Image();
+
+ private zeroingFaults: HTMLElement =
+ (document.getElementById('zeroing_faults') as HTMLElement);
+
+ private leftDrivetrainEncoder: HTMLElement =
+ (document.getElementById('left_drivetrain_encoder') as HTMLElement);
+ private rightDrivetrainEncoder: HTMLElement =
+ (document.getElementById('right_drivetrain_encoder') as HTMLElement);
+ private falconRightFrontPosition: HTMLElement =
+ (document.getElementById('falcon_right_front') as HTMLElement);
+ private falconRightBackPosition: HTMLElement =
+ (document.getElementById('falcon_right_back') as HTMLElement);
+ private falconLeftFrontPosition: HTMLElement =
+ (document.getElementById('falcon_left_front') as HTMLElement);
+ private falconLeftBackPosition: HTMLElement =
+ (document.getElementById('falcon_left_back') as HTMLElement);
+
+ constructor(private readonly connection: Connection) {
+ (document.getElementById('field') as HTMLElement).appendChild(this.canvas);
+
+ this.fieldImage.src = '2024.png';
+
+ // Construct a table header.
+ {
+ const row = document.createElement('div');
+ const nameCell = document.createElement('div');
+ nameCell.innerHTML = 'Rejection Reason';
+ row.appendChild(nameCell);
+ for (const camera of CAMERAS) {
+ const nodeCell = document.createElement('div');
+ nodeCell.innerHTML = camera;
+ row.appendChild(nodeCell);
+ }
+ document.getElementById('vision_readouts').appendChild(row);
+ }
+
+ // Add rejection reason row for aprilrobotics rejections.
+ {
+ const row = document.createElement('div');
+ const nameCell = document.createElement('div');
+ nameCell.innerHTML = 'Rejected by aprilrobotics';
+ row.appendChild(nameCell);
+ this.rejectionReasonCells.push([]);
+ for (const camera of CAMERAS) {
+ const valueCell = document.createElement('div');
+ valueCell.innerHTML = 'NA';
+ this.rejectionReasonCells[this.rejectionReasonCells.length - 1].push(
+ valueCell);
+ row.appendChild(valueCell);
+ }
+ document.getElementById('vision_readouts').appendChild(row);
+ }
+
+ for (let ii = 0; ii < CAMERA_COLORS.length; ++ii) {
+ const legendEntry = document.createElement('div');
+ legendEntry.style.color = CAMERA_COLORS[ii];
+ legendEntry.innerHTML = CAMERAS[ii];
+ document.getElementById('legend').appendChild(legendEntry);
+ }
+
+ this.connection.addConfigHandler(() => {
+ // Visualization message is reliable so that we can see *all* the vision
+ // matches.
+
+ for (const camera in CAMERAS) {
+ // Make unreliable to reduce network spam.
+ this.connection.addHandler(
+ CAMERAS[camera], 'frc971.vision.TargetMap', (data) => {
+ this.handleCameraTargetMap(camera, data);
+ });
+ }
+
+ this.connection.addHandler(
+ '/superstructure', "y2024_bot3.control_loops.superstructure.Status",
+ (data) => {
+ this.handleSuperstructureStatus(data)
+ });
+ this.connection.addHandler(
+ '/superstructure', "y2024_bot3.control_loops.superstructure.Positon",
+ (data) => {
+ this.handleSuperstructurePosition(data)
+ });
+ this.connection.addHandler(
+ '/aos', 'aos.message_bridge.ServerStatistics',
+ (data) => {this.handleServerStatistics(data)});
+ this.connection.addHandler(
+ '/aos', 'aos.message_bridge.ClientStatistics',
+ (data) => {this.handleClientStatistics(data)});
+ });
+ }
+ private handleCameraTargetMap(pi: string, data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const targetMap = TargetMap.getRootAsTargetMap(fbBuffer);
+ this.rejectionReasonCells[this.rejectionReasonCells.length - 1][pi]
+ .innerHTML = targetMap.rejections().toString();
+ }
+
+ private handleSuperstructureStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.superstructureStatus = SuperstructureStatus.getRootAsStatus(fbBuffer);
+ }
+
+ private handleSuperstructurePosition(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.superstructurePosition = SuperstructurePosition.getRootAsPosition(fbBuffer);
+ }
+
+ private populateNodeConnections(nodeName: string): void {
+ const row = document.createElement('div');
+ this.messageBridgeDiv.appendChild(row);
+ const nodeDiv = document.createElement('div');
+ nodeDiv.innerHTML = nodeName;
+ row.appendChild(nodeDiv);
+ const clientDiv = document.createElement('div');
+ clientDiv.innerHTML = 'N/A';
+ row.appendChild(clientDiv);
+ const serverDiv = document.createElement('div');
+ serverDiv.innerHTML = 'N/A';
+ row.appendChild(serverDiv);
+ this.serverStatuses.set(nodeName, serverDiv);
+ this.clientStatuses.set(nodeName, clientDiv);
+ }
+
+ private setCurrentNodeState(element: HTMLElement, state: ConnectionState):
+ void {
+ if (state === ConnectionState.CONNECTED) {
+ element.innerHTML = ConnectionState[state];
+ element.classList.remove('faulted');
+ element.classList.add('connected');
+ } else {
+ element.innerHTML = ConnectionState[state];
+ element.classList.remove('connected');
+ element.classList.add('faulted');
+ }
+ }
+
+ private handleServerStatistics(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const serverStatistics =
+ ServerStatistics.getRootAsServerStatistics(fbBuffer);
+
+ for (let ii = 0; ii < serverStatistics.connectionsLength(); ++ii) {
+ const connection = serverStatistics.connections(ii);
+ const nodeName = connection.node().name();
+ if (!this.serverStatuses.has(nodeName)) {
+ this.populateNodeConnections(nodeName);
+ }
+ this.setCurrentNodeState(
+ this.serverStatuses.get(nodeName), connection.state());
+ }
+ }
+
+ private handleClientStatistics(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const clientStatistics =
+ ClientStatistics.getRootAsClientStatistics(fbBuffer);
+
+ for (let ii = 0; ii < clientStatistics.connectionsLength(); ++ii) {
+ const connection = clientStatistics.connections(ii);
+ const nodeName = connection.node().name();
+ if (!this.clientStatuses.has(nodeName)) {
+ this.populateNodeConnections(nodeName);
+ }
+ this.setCurrentNodeState(
+ this.clientStatuses.get(nodeName), connection.state());
+ }
+ }
+
+ drawField(): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.scale(1.0, -1.0);
+ ctx.drawImage(
+ this.fieldImage, 0, 0, this.fieldImage.width, this.fieldImage.height,
+ -FIELD_EDGE_X, -FIELD_SIDE_Y, FIELD_LENGTH, FIELD_WIDTH);
+ ctx.restore();
+ }
+
+ drawCamera(x: number, y: number, theta: number, color: string = 'blue'):
+ void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.strokeStyle = color;
+ ctx.beginPath();
+ ctx.moveTo(0.5, 0.5);
+ ctx.lineTo(0, 0);
+ ctx.lineTo(0.5, -0.5);
+ ctx.stroke();
+ ctx.beginPath();
+ ctx.arc(0, 0, 0.25, -Math.PI / 4, Math.PI / 4);
+ ctx.stroke();
+ ctx.restore();
+ }
+
+ drawRobot(
+ x: number, y: number, theta: number, color: string = 'blue',
+ dashed: boolean = false): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.strokeStyle = color;
+ ctx.lineWidth = ROBOT_WIDTH / 10.0;
+ if (dashed) {
+ ctx.setLineDash([0.05, 0.05]);
+ } else {
+ // Empty array = solid line.
+ ctx.setLineDash([]);
+ }
+ ctx.rect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);
+ ctx.stroke();
+
+ // Draw line indicating which direction is forwards on the robot.
+ ctx.beginPath();
+ ctx.moveTo(0, 0);
+ ctx.lineTo(ROBOT_LENGTH / 2.0, 0);
+ ctx.stroke();
+
+ ctx.restore();
+}
+
+ setZeroing(div: HTMLElement): void {
+ div.innerHTML = 'zeroing';
+ div.classList.remove('faulted');
+ div.classList.add('zeroing');
+ div.classList.remove('near');
+ }
+
+ setEstopped(div: HTMLElement): void {
+ div.innerHTML = 'estopped';
+ div.classList.add('faulted');
+ div.classList.remove('zeroing');
+ div.classList.remove('near');
+ }
+
+ setTargetValue(
+ div: HTMLElement, target: number, val: number, tolerance: number): void {
+ div.innerHTML = val.toFixed(4);
+ div.classList.remove('faulted');
+ div.classList.remove('zeroing');
+ if (Math.abs(target - val) < tolerance) {
+ div.classList.add('near');
+ } else {
+ div.classList.remove('near');
+ }
+ }
+
+ setValue(div: HTMLElement, val: number): void {
+ div.innerHTML = val.toFixed(4);
+ div.classList.remove('faulted');
+ div.classList.remove('zeroing');
+ div.classList.remove('near');
+ }
+
+ setBoolean(div: HTMLElement, triggered: boolean): void {
+ div.innerHTML = ((triggered) ? "TRUE" : "FALSE")
+ div.className = '';
+ if (triggered) {
+ div.classList.add('lightgreen');
+ } else {
+ div.classList.add('lightcoral');
+ }
+ }
+
+ draw(): void {
+ this.reset();
+ this.drawField();
+
+ window.requestAnimationFrame(() => this.draw());
+ }
+
+ reset(): void {
+ const ctx = this.canvas.getContext('2d');
+ // Empty space from the canvas boundary to the image
+ const IMAGE_PADDING = 10;
+ ctx.setTransform(1, 0, 0, 1, 0, 0);
+ const size = window.innerHeight * 0.9;
+ ctx.canvas.height = size;
+ const width = size / 2 + 20;
+ ctx.canvas.width = width;
+ ctx.clearRect(0, 0, size, width);
+
+ // Translate to center of display.
+ ctx.translate(width / 2, size / 2);
+ // Coordinate system is:
+ // x -> forward.
+ // y -> to the left.
+ ctx.rotate(-Math.PI / 2);
+ ctx.scale(1, -1);
+
+ const M_TO_PX = (size - IMAGE_PADDING) / FIELD_LENGTH;
+ ctx.scale(M_TO_PX, M_TO_PX);
+ ctx.lineWidth = 1 / M_TO_PX;
+ }
+}
diff --git a/y2024_bot3/www/field_main.ts b/y2024_bot3/www/field_main.ts
new file mode 100644
index 0000000..ef8b99d
--- /dev/null
+++ b/y2024_bot3/www/field_main.ts
@@ -0,0 +1,11 @@
+import {Connection} from '../../aos/network/www/proxy';
+
+import {FieldHandler} from './field_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const fieldHandler = new FieldHandler(conn);
+
+fieldHandler.draw();
diff --git a/y2024_bot3/www/index.html b/y2024_bot3/www/index.html
new file mode 100644
index 0000000..98ecf42
--- /dev/null
+++ b/y2024_bot3/www/index.html
@@ -0,0 +1,7 @@
+<html>
+ <body>
+ <a href="field.html">Field Visualization</a><br>
+ <a href="plotter.html">Plots</a><br>
+ <a href="starter.html">AOS Starter Status</a>
+ </body>
+</html>
diff --git a/y2024_bot3/www/plotter.html b/y2024_bot3/www/plotter.html
new file mode 100644
index 0000000..86f5aa8
--- /dev/null
+++ b/y2024_bot3/www/plotter.html
@@ -0,0 +1,8 @@
+<html>
+ <head>
+ <script src="plot_index_bundle.min.js" defer></script>
+ <link rel="stylesheet" href="styles.css">
+ </head>
+ <body>
+ </body>
+</html>
diff --git a/y2024_bot3/y2024_bot3.json b/y2024_bot3/y2024_bot3.json
new file mode 100644
index 0000000..aea954c
--- /dev/null
+++ b/y2024_bot3/y2024_bot3.json
@@ -0,0 +1,19 @@
+{
+ "channel_storage_duration": 10000000000,
+ "maps": [
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.RobotState"
+ },
+ "rename": {
+ "name": "/roborio/aos"
+ }
+ }
+ ],
+ "imports": [
+ "y2024_bot3_roborio.json",
+ "y2024_bot3_imu.json",
+ "y2024_bot3_orin1.json",
+ ]
+}
diff --git a/y2024_bot3/y2024_bot3_imu.json b/y2024_bot3/y2024_bot3_imu.json
new file mode 100644
index 0000000..9473c59
--- /dev/null
+++ b/y2024_bot3/y2024_bot3_imu.json
@@ -0,0 +1,547 @@
+{
+ "channels": [
+ {
+ "name": "/imu/aos",
+ "type": "aos.util.FilesystemStatus",
+ "source_node": "imu",
+ "frequency": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.JoystickState",
+ "source_node": "imu",
+ "frequency": 100,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "orin1"
+ ],
+ "destination_nodes": [
+ {
+ "name": "orin1",
+ "priority": 5,
+ "time_to_live": 50000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ]
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/orin1/imu/aos/aos-JoystickState",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 300,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.timing.Report",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 30,
+ "max_size": 10912
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "imu",
+ "frequency": 200,
+ "num_senders": 30
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.Status",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 6144
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "imu",
+ "max_size": 2048,
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "imu",
+ "frequency": 20,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "imu",
+ "frequency": 15,
+ "num_senders": 2,
+ "logger_nodes": [
+ "roborio"
+ ],
+ "max_size": 400,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/roborio/imu/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "imu",
+ "max_size": 208
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-StarterRpc",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/imu/camera0",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "imu",
+ "channel_storage_duration": 1000000000,
+ "frequency": 70,
+ "max_size": 4752384,
+ "num_readers": 6,
+ "read_method": "PIN",
+ "num_senders": 18
+ },
+ {
+ "name": "/imu/camera1",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "imu",
+ "channel_storage_duration": 1000000000,
+ "frequency": 70,
+ "max_size": 4752384,
+ "num_readers": 6,
+ "read_method": "PIN",
+ "num_senders": 18
+ },
+ {
+ "name": "/imu/camera0",
+ "type": "foxglove.CompressedImage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "channel_storage_duration": 1000000000,
+ "frequency": 70,
+ "max_size": 622384
+ },
+ {
+ "name": "/imu/camera1",
+ "type": "foxglove.CompressedImage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "channel_storage_duration": 1000000000,
+ "frequency": 70,
+ "max_size": 622384
+ },
+ {
+ "name": "/imu/camera0",
+ "type": "foxglove.ImageAnnotations",
+ "source_node": "imu",
+ "frequency": 70,
+ "max_size": 50000
+ },
+ {
+ "name": "/imu/camera1",
+ "type": "foxglove.ImageAnnotations",
+ "source_node": "imu",
+ "frequency": 70,
+ "max_size": 50000
+ },
+ {
+ "name": "/imu/camera0",
+ "type": "frc971.vision.TargetMap",
+ "source_node": "imu",
+ "frequency": 70,
+ "num_senders": 2,
+ "max_size": 1024
+ },
+ {
+ "name": "/imu/camera1",
+ "type": "frc971.vision.TargetMap",
+ "source_node": "imu",
+ "frequency": 70,
+ "num_senders": 2,
+ "max_size": 1024
+ },
+ {
+ "name": "/imu",
+ "type": "frc971.imu.DualImu",
+ "source_node": "imu",
+ "frequency": 1100,
+ "num_senders": 1,
+ "max_size": 496
+ },
+ {
+ "name": "/imu",
+ "type": "frc971.imu.CanTranslatorStatus",
+ "source_node": "imu",
+ "frequency": 1000,
+ "num_senders": 1,
+ "max_size": 200
+ },
+ {
+ "name": "/can/cana",
+ "type": "frc971.can_logger.CanFrame",
+ "source_node": "imu",
+ "frequency": 9000,
+ "channel_storage_duration": 7000000000,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/imu",
+ "type": "frc971.imu.DualImuBlenderStatus",
+ "source_node": "imu",
+ "frequency": 1100,
+ "num_senders": 1,
+ "max_size": 200
+ },
+ {
+ "name": "/imu/hardware_monitor",
+ "type": "frc971.orin.HardwareStats",
+ "source_node": "imu",
+ "frequency": 2
+ },
+ {
+ "name": "/imu/constants",
+ "type": "y2024_bot3.Constants",
+ "source_node": "imu",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 65536
+ }
+ ],
+ "applications": [
+ {
+ "name": "message_bridge_client",
+ "args": [
+ "--rt_priority=16",
+ "--sinit_max_init_timeout=5000",
+ "--rmem=8388608"
+ ],
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "irq_affinity",
+ "executable_name": "irq_affinity",
+ "user": "root",
+ "args": ["--user=pi", "--irq_config=orin_irq_config.json"],
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "filesystem_monitor",
+ "executable_name": "filesystem_monitor",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "hardware_monitor",
+ "executable_name": "hardware_monitor",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "joystick_republish",
+ "executable_name": "joystick_republish",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "user": "pi",
+ "args": [
+ "--rt_priority=16",
+ "--force_wmem_max=131072"
+ ],
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "imu_can_logger",
+ "executable_name": "can_logger",
+ "args": [
+ "--priority=59",
+ "--affinity=5"
+ ],
+ "nodes": [
+ "imu"
+ ]
+ },
+ // TODO(max): Update the channel value with whatever channel the IMU is on.
+ {
+ "name": "can_translator",
+ "executable_name": "can_translator",
+ "args": [
+ "--channel=/can/cana"
+ ],
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "dual_imu_blender",
+ "executable_name": "dual_imu_blender",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "web_proxy",
+ "executable_name": "web_proxy_main",
+ "args": [
+ "--min_ice_port=5800",
+ "--max_ice_port=5810"
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "foxglove_websocket",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "image_logger",
+ "executable_name": "image_logger",
+ "args": [
+ "--rotate_every",
+ "30.0",
+ "--direct",
+ "--flush_size=4194304"
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "foxglove_websocket",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "foxglove_image_converter0",
+ "executable_name": "foxglove_image_converter",
+ "user": "pi",
+ "args": [
+ "--channel", "/camera0"
+ ],
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "foxglove_image_converter1",
+ "executable_name": "foxglove_image_converter",
+ "user": "pi",
+ "args": [
+ "--channel", "/camera1"
+ ],
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "argus_monitor_imu",
+ "executable_name": "argus_monitor",
+ "args": [
+ "/imu/camera0",
+ "frc971.vision.TargetMap",
+ "/imu/camera1",
+ "frc971.vision.TargetMap",
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "argus_camera0",
+ "executable_name": "argus_camera",
+ "args": [
+ "--camera=0",
+ "--channel=/camera0"
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "argus_camera1",
+ "executable_name": "argus_camera",
+ "args": [
+ "--camera=1",
+ "--channel=/camera1"
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "apriltag_detector0",
+ "executable_name": "apriltag_detector",
+ "args": [
+ "--channel=/camera0"
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "apriltag_detector1",
+ "executable_name": "apriltag_detector",
+ "args": [
+ "--channel=/camera1"
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/constants*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/constants"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/aos"
+ }
+ },
+ {
+ "match": {
+ "name": "/camera*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/camera"
+ }
+ },
+ {
+ "match": {
+ "name": "/hardware_monitor*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/hardware_monitor"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "imu",
+ "hostname": "orin2",
+ "hostnames": [
+ "orin-971-2",
+ "orin-7971-2",
+ "orin-8971-2",
+ "orin-9971-2"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "roborio"
+ },
+ {
+ "name": "orin1"
+ }
+ ]
+}
diff --git a/y2024_bot3/y2024_bot3_orin1.json b/y2024_bot3/y2024_bot3_orin1.json
new file mode 100644
index 0000000..8d5085e
--- /dev/null
+++ b/y2024_bot3/y2024_bot3_orin1.json
@@ -0,0 +1,493 @@
+{
+ "channels": [
+ {
+ "name": "/orin1/aos",
+ "type": "aos.util.FilesystemStatus",
+ "source_node": "orin1",
+ "frequency": 2
+ },
+ {
+ "name": "/orin1/aos",
+ "type": "aos.timing.Report",
+ "source_node": "orin1",
+ "frequency": 50,
+ "num_senders": 30,
+ "max_size": 8192
+ },
+ {
+ "name": "/orin1/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "orin1",
+ "frequency": 200,
+ "num_senders": 30
+ },
+ {
+ "name": "/orin1/aos",
+ "type": "aos.starter.Status",
+ "source_node": "orin1",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 4096
+ },
+ {
+ "name": "/orin1/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "orin1",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/orin1/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "orin1",
+ "max_size": 2048,
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/orin1/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "orin1",
+ "frequency": 20,
+ "num_senders": 2
+ },
+ {
+ "name": "/orin1/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "orin1",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/orin1/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "orin1",
+ "frequency": 15,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "max_size": 200,
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "orin1"
+ ]
+ }
+ ]
+ },
+ {
+ "name": "/orin1/aos/remote_timestamps/imu/orin1/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "orin1",
+ "max_size": 208
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "imu",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "orin1"
+ ],
+ "destination_nodes": [
+ {
+ "name": "orin1",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/orin1/imu/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "imu",
+ "max_size": 208
+ },
+ {
+ "name": "/orin1/camera0",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "orin1",
+ "channel_storage_duration": 1000000000,
+ "frequency": 70,
+ "max_size": 4752384,
+ "num_readers": 6,
+ "read_method": "PIN",
+ "num_senders": 18
+ },
+ {
+ "name": "/orin1/camera1",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "orin1",
+ "channel_storage_duration": 1000000000,
+ "frequency": 70,
+ "max_size": 4752384,
+ "num_readers": 6,
+ "read_method": "PIN",
+ "num_senders": 18
+ },
+ {
+ "name": "/orin1/camera0",
+ "type": "foxglove.CompressedImage",
+ "source_node": "orin1",
+ "logger": "NOT_LOGGED",
+ "channel_storage_duration": 1000000000,
+ "frequency": 70,
+ "max_size": 622384
+ },
+ {
+ "name": "/orin1/camera1",
+ "type": "foxglove.CompressedImage",
+ "source_node": "orin1",
+ "logger": "NOT_LOGGED",
+ "channel_storage_duration": 1000000000,
+ "frequency": 70,
+ "max_size": 622384
+ },
+ {
+ "name": "/orin1/camera0",
+ "type": "foxglove.ImageAnnotations",
+ "source_node": "orin1",
+ "frequency": 70,
+ "max_size": 50000
+ },
+ {
+ "name": "/orin1/camera1",
+ "type": "foxglove.ImageAnnotations",
+ "source_node": "orin1",
+ "frequency": 70,
+ "max_size": 50000
+ },
+ {
+ "name": "/orin1/camera0",
+ "type": "frc971.vision.TargetMap",
+ "source_node": "orin1",
+ "frequency": 70,
+ "num_senders": 2,
+ "max_size": 1024,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 4,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "orin1"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/orin1/camera1",
+ "type": "frc971.vision.TargetMap",
+ "source_node": "orin1",
+ "frequency": 70,
+ "num_senders": 2,
+ "max_size": 1024,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 4,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "orin1"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/orin1/aos/remote_timestamps/imu/orin1/camera0/frc971-vision-TargetMap",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 80,
+ "source_node": "orin1",
+ "max_size": 208
+ },
+ {
+ "name": "/orin1/aos/remote_timestamps/imu/orin1/camera1/frc971-vision-TargetMap",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 80,
+ "source_node": "orin1",
+ "max_size": 208
+ },
+ {
+ "name": "/orin1/hardware_monitor",
+ "type": "frc971.orin.HardwareStats",
+ "source_node": "orin1",
+ "frequency": 2
+ },
+ {
+ "name": "/orin1/constants",
+ "type": "y2024_bot3.Constants",
+ "source_node": "orin1",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 65536
+ }
+ ],
+ "applications": [
+ {
+ "name": "message_bridge_client",
+ "executable_name": "message_bridge_client",
+ "args": [
+ "--rt_priority=16",
+ "--sinit_max_init_timeout=5000",
+ "--rmem=8388608"
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "irq_affinity",
+ "executable_name": "irq_affinity",
+ "user": "root",
+ "args": ["--user=pi", "--irq_config=orin_irq_config.json"],
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "filesystem_monitor",
+ "executable_name": "filesystem_monitor",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "hardware_monitor",
+ "executable_name": "hardware_monitor",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "args": [
+ "--rt_priority=16",
+ "--force_wmem_max=131072"
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "web_proxy",
+ "executable_name": "web_proxy_main",
+ "user": "pi",
+ "args": [
+ "--min_ice_port=5800",
+ "--max_ice_port=5810"
+ ],
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "image_logger",
+ "executable_name": "image_logger",
+ "args": [
+ "--rotate_every",
+ "30.0",
+ "--direct",
+ "--flush_size=4194304"
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "foxglove_websocket",
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "foxglove_image_converter0",
+ "executable_name": "foxglove_image_converter",
+ "user": "pi",
+ "args": [
+ "--channel", "/camera0"
+ ],
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "foxglove_image_converter1",
+ "executable_name": "foxglove_image_converter",
+ "user": "pi",
+ "args": [
+ "--channel", "/camera1"
+ ],
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "argus_monitor_orin1",
+ "executable_name": "argus_monitor",
+ "args": [
+ "/orin1/camera0",
+ "frc971.vision.TargetMap",
+ "/orin1/camera1",
+ "frc971.vision.TargetMap",
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "argus_camera0",
+ "executable_name": "argus_camera",
+ "args": [
+ "--camera=0",
+ "--channel=/camera0"
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "argus_camera1",
+ "executable_name": "argus_camera",
+ "args": [
+ "--camera=1",
+ "--channel=/camera1"
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "apriltag_detector0",
+ "executable_name": "apriltag_detector",
+ "args": [
+ "--channel=/camera0"
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "apriltag_detector1",
+ "executable_name": "apriltag_detector",
+ "args": [
+ "--channel=/camera1"
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ },
+ {
+ "name": "image_streamer",
+ "executable_name": "image_streamer",
+ "args": [
+ "--device=/dev/uvcvideo",
+ "--height=480",
+ "--width=640",
+ "--nopublish_images",
+ "--exposure=0",
+ "--framerate=30",
+ "--streaming_port=1181",
+ "--bitrate=1000000",
+ "--data_dir=/home/pi/bin/image_streamer_www"
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin1"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "orin1"
+ },
+ "rename": {
+ "name": "/orin1/aos"
+ }
+ },
+ {
+ "match": {
+ "name": "/constants*",
+ "source_node": "orin1"
+ },
+ "rename": {
+ "name": "/orin1/constants"
+ }
+ },
+ {
+ "match": {
+ "name": "/camera*",
+ "source_node": "orin1"
+ },
+ "rename": {
+ "name": "/orin1/camera"
+ }
+ },
+ {
+ "match": {
+ "name": "/hardware_monitor*",
+ "source_node": "orin1"
+ },
+ "rename": {
+ "name": "/orin1/hardware_monitor"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "orin1",
+ "hostname": "orin1",
+ "hostnames": [
+ "orin-971-1",
+ "orin-7971-1",
+ "orin-8971-1",
+ "orin-9971-1"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "imu"
+ }
+ ]
+}
diff --git a/y2024_bot3/y2024_bot3_roborio.json b/y2024_bot3/y2024_bot3_roborio.json
new file mode 100644
index 0000000..ea5e547
--- /dev/null
+++ b/y2024_bot3/y2024_bot3_roborio.json
@@ -0,0 +1,336 @@
+{
+ "channels": [
+ {
+ "name": "/roborio/aos",
+ "type": "aos.JoystickState",
+ "source_node": "roborio",
+ "frequency": 100,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "time_to_live": 50000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "frc971.PDPValues",
+ "source_node": "roborio",
+ "frequency": 55,
+ "max_size": 368
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.RobotState",
+ "source_node": "roborio",
+ "frequency": 250
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.timing.Report",
+ "source_node": "roborio",
+ "frequency": 50,
+ "num_senders": 30,
+ "max_size": 8192
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "roborio",
+ "frequency": 500,
+ "max_size": 1000,
+ "num_senders": 20
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.Status",
+ "source_node": "roborio",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2000
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "frequency": 10,
+ "max_size": 400,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "roborio",
+ "max_size": 2048,
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "roborio",
+ "frequency": 20,
+ "max_size": 2000,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "roborio",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "roborio",
+ "max_size": 208
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "roborio",
+ "frequency": 15,
+ "num_senders": 2,
+ "max_size": 512,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2024_bot3.control_loops.superstructure.Goal",
+ "source_node": "roborio",
+ "frequency": 250,
+ "max_size": 512
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2024_bot3.control_loops.superstructure.Status",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 2048,
+ "num_senders": 2
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2024_bot3.control_loops.superstructure.Output",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 2,
+ "max_size": 224
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2024_bot3.control_loops.superstructure.Position",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 2,
+ "max_size": 448
+ },
+ {
+ "name": "/superstructure/canivore",
+ "type": "y2024_bot3.control_loops.superstructure.CANPosition",
+ "source_node": "roborio",
+ "frequency": 220,
+ "num_senders": 2,
+ "max_size": 1024
+ },
+ {
+ "name": "/superstructure/rio",
+ "type": "y2024_bot3.control_loops.superstructure.CANPosition",
+ "source_node": "roborio",
+ "frequency": 220,
+ "num_senders": 2,
+ "max_size": 1024
+ },
+ {
+ "name": "/can",
+ "type": "frc971.can_logger.CanFrame",
+ "source_node": "roborio",
+ "frequency": 6000,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/roborio",
+ "type": "frc971.CANConfiguration",
+ "source_node": "roborio",
+ "frequency": 2
+ },
+ {
+ "name": "/roborio/constants",
+ "type": "y2024_bot3.Constants",
+ "source_node": "roborio",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 65536
+ }
+ ],
+ "applications": [
+ {
+ "name": "trajectory_generator",
+ "executable_name": "trajectory_generator",
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "superstructure",
+ "executable_name": "superstructure",
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_irq_affinity",
+ "executable_name": "irq_affinity",
+ "args": [
+ "--irq_config=/home/admin/bin/roborio_irq_config.json"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "joystick_reader",
+ "executable_name": "joystick_reader",
+ "args": [
+ "--nodie_on_malloc"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "wpilib_interface",
+ "executable_name": "wpilib_interface",
+ "args": [
+ "--nodie_on_malloc"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_web_proxy",
+ "executable_name": "web_proxy_main",
+ "args": [
+ "--min_ice_port=5800",
+ "--max_ice_port=5810"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_message_bridge_client",
+ "executable_name": "message_bridge_client",
+ "args": [
+ "--rt_priority=16",
+ "--sinit_max_init_timeout=5000",
+ "--rmem=2097152"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "args": [
+ "--rt_priority=16",
+ "--force_wmem_max=131072"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "logger",
+ "executable_name": "logger_main",
+ "args": [
+ "--snappy_compress",
+ "--logging_folder=/home/admin/logs/",
+ "--rotate_every", "30.0"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "constants_sender_roborio",
+ "executable_name": "constants_sender",
+ "autorestart": false,
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_can_logger",
+ "executable_name": "can_logger",
+ "autostart": false,
+ "args": [
+ "--poll"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/constants*",
+ "source_node": "roborio"
+ },
+ "rename": {
+ "name": "/roborio/constants"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "roborio"
+ },
+ "rename": {
+ "name": "/roborio/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "roborio",
+ "hostname": "roborio",
+ "hostnames": [
+ "roboRIO-971-FRC",
+ "roboRIO-6971-FRC",
+ "roboRIO-7971-FRC",
+ "roboRIO-8971-FRC",
+ "roboRIO-9971-FRC"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "imu"
+ }
+ ]
+}