Merge "Fix logic for handling beambreak value in extend"
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index b179ad9..9c236c8 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -31,6 +31,9 @@
"//y2023/control_loops/superstructure:superstructure_plotter",
"//y2023/localizer:corrections_plotter",
"//y2023/localizer:localizer_plotter",
+ "//y2024/control_loops/superstructure:superstructure_plotter",
+ "//y2024/localizer:corrections_plotter",
+ "//y2024/localizer:localizer_plotter",
],
)
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 1ee85d1..610ac4b 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -21,46 +21,32 @@
// using JSON rather than requiring people to write a script just to create
// a plot.
import {Configuration} from '../../aos/configuration_generated';
-import {Connection} from '../../aos/network/www/proxy';
-import {plotImu} from '../wpilib/imu_plotter';
-import {plotDrivetrain} from '../control_loops/drivetrain/drivetrain_plotter';
-import {plotSpline} from '../control_loops/drivetrain/spline_plotter';
-import {plotDownEstimator} from '../control_loops/drivetrain/down_estimator_plotter';
-import {plotRobotState} from
- '../control_loops/drivetrain/robot_state_plotter'
-import {plotFinisher as plot2020Finisher} from
- '../../y2020/control_loops/superstructure/finisher_plotter'
-import {plotTurret as plot2020Turret} from
- '../../y2020/control_loops/superstructure/turret_plotter'
-import {plotLocalizer as plot2020Localizer} from
- '../../y2020/control_loops/drivetrain/localizer_plotter'
-import {plotAccelerator as plot2020Accelerator} from
- '../../y2020/control_loops/superstructure/accelerator_plotter'
-import {plotHood as plot2020Hood} from
- '../../y2020/control_loops/superstructure/hood_plotter'
-import {plotSuperstructure as plot2021Superstructure} from
- '../../y2021_bot3/control_loops/superstructure/superstructure_plotter';
-import {plotTurret as plot2022Turret} from
- '../../y2022/control_loops/superstructure/turret_plotter'
-import {plotSuperstructure as plot2022Superstructure} from
- '../../y2022/control_loops/superstructure/superstructure_plotter'
-import {plotSuperstructure as plot2023Superstructure} from
- '../../y2023/control_loops/superstructure/superstructure_plotter'
-import {plotCatapult as plot2022Catapult} from
- '../../y2022/control_loops/superstructure/catapult_plotter'
-import {plotIntakeFront as plot2022IntakeFront, plotIntakeBack as plot2022IntakeBack} from
- '../../y2022/control_loops/superstructure/intake_plotter'
-import {plotClimber as plot2022Climber} from
- '../../y2022/control_loops/superstructure/climber_plotter'
-import {plotLocalizer as plot2022Localizer} from
- '../../y2022/localizer/localizer_plotter'
-import {plotLocalizer as plot2023Localizer} from
- '../../y2023/localizer/localizer_plotter'
-import {plotVision as plot2022Vision} from
- '../../y2022/vision/vision_plotter'
-import {plotVision as plot2023Corrections} from
- '../../y2023/localizer/corrections_plotter'
import {plotDemo} from '../../aos/network/www/demo_plot';
+import {Connection} from '../../aos/network/www/proxy';
+import {plotLocalizer as plot2020Localizer} from '../../y2020/control_loops/drivetrain/localizer_plotter'
+import {plotAccelerator as plot2020Accelerator} from '../../y2020/control_loops/superstructure/accelerator_plotter'
+import {plotFinisher as plot2020Finisher} from '../../y2020/control_loops/superstructure/finisher_plotter'
+import {plotHood as plot2020Hood} from '../../y2020/control_loops/superstructure/hood_plotter'
+import {plotTurret as plot2020Turret} from '../../y2020/control_loops/superstructure/turret_plotter'
+import {plotSuperstructure as plot2021Superstructure} from '../../y2021_bot3/control_loops/superstructure/superstructure_plotter';
+import {plotCatapult as plot2022Catapult} from '../../y2022/control_loops/superstructure/catapult_plotter'
+import {plotClimber as plot2022Climber} from '../../y2022/control_loops/superstructure/climber_plotter'
+import {plotIntakeBack as plot2022IntakeBack, plotIntakeFront as plot2022IntakeFront} from '../../y2022/control_loops/superstructure/intake_plotter'
+import {plotSuperstructure as plot2022Superstructure} from '../../y2022/control_loops/superstructure/superstructure_plotter'
+import {plotTurret as plot2022Turret} from '../../y2022/control_loops/superstructure/turret_plotter'
+import {plotLocalizer as plot2022Localizer} from '../../y2022/localizer/localizer_plotter'
+import {plotVision as plot2022Vision} from '../../y2022/vision/vision_plotter'
+import {plotSuperstructure as plot2023Superstructure} from '../../y2023/control_loops/superstructure/superstructure_plotter'
+import {plotVision as plot2023Corrections} from '../../y2023/localizer/corrections_plotter'
+import {plotLocalizer as plot2023Localizer} from '../../y2023/localizer/localizer_plotter'
+import {plotClimber as plot2024Climber, plotIntake as plot2024Intake, plotSuperstructure as plot2024Superstructure} from '../../y2024/control_loops/superstructure/superstructure_plotter'
+import {plotVision as plot2024Corrections} from '../../y2024/localizer/corrections_plotter'
+import {plotLocalizer as plot2024Localizer} from '../../y2024/localizer/localizer_plotter'
+import {plotDownEstimator} from '../control_loops/drivetrain/down_estimator_plotter';
+import {plotDrivetrain} from '../control_loops/drivetrain/drivetrain_plotter';
+import {plotRobotState} from '../control_loops/drivetrain/robot_state_plotter'
+import {plotSpline} from '../control_loops/drivetrain/spline_plotter';
+import {plotImu} from '../wpilib/imu_plotter';
const rootDiv = document.createElement('div');
rootDiv.style.width = '100%';
@@ -118,6 +104,11 @@
['Spline Debug', new PlotState(plotDiv, plotSpline)],
['Down Estimator', new PlotState(plotDiv, plotDownEstimator)],
['Robot State', new PlotState(plotDiv, plotRobotState)],
+ ['2024 Vision', new PlotState(plotDiv, plot2024Corrections)],
+ ['2024 Localizer', new PlotState(plotDiv, plot2024Localizer)],
+ ['2024 Superstructure', new PlotState(plotDiv, plot2024Superstructure)],
+ ['2024 Climber', new PlotState(plotDiv, plot2024Climber)],
+ ['2024 Intake', new PlotState(plotDiv, plot2024Intake)],
['2023 Vision', new PlotState(plotDiv, plot2023Corrections)],
['2023 Localizer', new PlotState(plotDiv, plot2023Localizer)],
['2023 Superstructure', new PlotState(plotDiv, plot2023Superstructure)],
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index f784f2e..2f89a0c 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -757,8 +757,9 @@
aos::monotonic_clock::time_point t) {
CHECK(!observations_.empty());
if (!observations_.full() && t < observations_.begin()->t) {
- LOG(ERROR) << "Dropped an observation that was received before we "
- "initialized.\n";
+ AOS_LOG(ERROR,
+ "Dropped an observation that was received before we "
+ "initialized.\n");
return;
}
auto cur_it = observations_.PushFromBottom(
diff --git a/frc971/wpilib/can_sensor_reader.cc b/frc971/wpilib/can_sensor_reader.cc
index 54be05d..1f56b17 100644
--- a/frc971/wpilib/can_sensor_reader.cc
+++ b/frc971/wpilib/can_sensor_reader.cc
@@ -7,8 +7,10 @@
aos::EventLoop *event_loop,
std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry,
std::vector<std::shared_ptr<TalonFX>> talonfxs,
- std::function<void(ctre::phoenix::StatusCode status)> flatbuffer_callback)
+ std::function<void(ctre::phoenix::StatusCode status)> flatbuffer_callback,
+ SignalSync sync)
: event_loop_(event_loop),
+ sync_(sync),
signals_(signals_registry.begin(), signals_registry.end()),
talonfxs_(talonfxs),
flatbuffer_callback_(flatbuffer_callback) {
@@ -28,8 +30,12 @@
}
void CANSensorReader::Loop() {
- ctre::phoenix::StatusCode status =
- ctre::phoenix6::BaseStatusSignal::WaitForAll(20_ms, signals_);
+ ctre::phoenix::StatusCode status;
+ if (sync_ == SignalSync::kDoSync) {
+ status = ctre::phoenix6::BaseStatusSignal::WaitForAll(20_ms, signals_);
+ } else {
+ status = ctre::phoenix6::BaseStatusSignal::RefreshAll(signals_);
+ }
if (!status.IsOK()) {
AOS_LOG(ERROR, "Failed to read signals from talonfx motors: %s: %s",
diff --git a/frc971/wpilib/can_sensor_reader.h b/frc971/wpilib/can_sensor_reader.h
index 8eddfca..c79d7f2 100644
--- a/frc971/wpilib/can_sensor_reader.h
+++ b/frc971/wpilib/can_sensor_reader.h
@@ -12,18 +12,23 @@
namespace frc971::wpilib {
class CANSensorReader {
public:
+ enum class SignalSync {
+ kDoSync,
+ kNoSync,
+ };
CANSensorReader(
aos::EventLoop *event_loop,
std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry,
std::vector<std::shared_ptr<TalonFX>> talonfxs,
- std::function<void(ctre::phoenix::StatusCode status)>
- flatbuffer_callback);
+ std::function<void(ctre::phoenix::StatusCode status)> flatbuffer_callback,
+ SignalSync sync = SignalSync::kDoSync);
private:
void Loop();
aos::EventLoop *event_loop_;
+ const SignalSync sync_;
const std::vector<ctre::phoenix6::BaseStatusSignal *> signals_;
// This is a vector of talonfxs becuase we don't need to care
diff --git a/y2024/BUILD b/y2024/BUILD
index f9102f9..9e27ab2 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -1,6 +1,5 @@
load("//frc971:downloader.bzl", "robot_downloader")
load("//aos:config.bzl", "aos_config")
-load("//tools/build_rules:template.bzl", "jinja2_template")
load("//aos/util:config_validator_macro.bzl", "config_validator_test")
config_validator_test(
@@ -58,6 +57,8 @@
"//frc971/vision:intrinsics_calibration",
"//y2024/vision:viewer",
"//y2024/constants:constants_sender",
+ "//y2024/localizer:localizer_main",
+ "//y2024/localizer:localizer_logger",
"//y2024/vision:foxglove_image_converter",
],
data = [
@@ -104,7 +105,6 @@
deps = [
":config_imu",
":config_orin1",
- ":config_orin2",
":config_roborio",
],
)
@@ -121,8 +121,14 @@
"//y2024/constants:constants_fbs",
"//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/can_logger:can_logging_fbs",
+ "//y2024/localizer:status_fbs",
+ "//y2024/localizer:visualization_fbs",
"//aos/network:timestamp_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"],
@@ -149,6 +155,7 @@
"//y2024/control_loops/superstructure:superstructure_can_position_fbs",
"//y2024/control_loops/superstructure:superstructure_output_fbs",
"//y2024/control_loops/superstructure:superstructure_position_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//y2024/control_loops/superstructure:superstructure_status_fbs",
"//frc971/can_logger:can_logging_fbs",
],
@@ -161,45 +168,30 @@
],
)
-[
- aos_config(
- name = "config_" + orin,
- src = "y2024_" + orin + ".json",
- flatbuffers = [
- "//aos/network:message_bridge_client_fbs",
- "//aos/network:message_bridge_server_fbs",
- "//aos/network:timestamp_fbs",
- "//aos/network:remote_message_fbs",
- "//y2024/constants:constants_fbs",
- "//frc971/control_loops/drivetrain/localization:localizer_output_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",
- "//frc971/input:aos_config",
- ],
- )
- for orin in [
- "orin1",
- "orin2",
- ]
-]
-
-[
- jinja2_template(
- name = "y2024_orin" + str(num) + ".json",
- src = "y2024_orin_template.json",
- parameters = {"NUM": str(num)},
- target_compatible_with = ["@platforms//os:linux"],
- )
- for num in range(1, 3)
-]
+aos_config(
+ name = "config_orin1",
+ src = "y2024_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/constants:constants_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/vision:calibration_fbs",
+ "//y2024/localizer:visualization_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",
+ "//frc971/input:aos_config",
+ ],
+)
cc_library(
name = "constants",
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index e79ebaf..38b63d7 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -23,27 +23,27 @@
"robot": {
{% set _ = intake_pivot_zero.update(
{
- "measured_absolute_position" : 2.99648178674155
+ "measured_absolute_position" : 3.2990161941868
}
) %}
"intake_constants": {{ intake_pivot_zero | tojson(indent=2)}},
"climber_constants": {
{% set _ = climber_zero.update(
{
- "measured_absolute_position" : 0.0102739460232857
+ "measured_absolute_position" : 0.00260967415741875
}
) %}
"zeroing_constants": {{ climber_zero | tojson(indent=2)}},
- "potentiometer_offset": {{ -0.935529777248618 + 1.83632555414775 }}
+ "potentiometer_offset": {{ -0.935529777248618 + 1.83632555414775 + 0.0431080619919798 - 0.493015437796464 }}
},
"catapult_constants": {
{% set _ = catapult_zero.update(
{
- "measured_absolute_position" : 2.79735930670211
+ "measured_absolute_position" : 2.8342002173381
}
) %}
"zeroing_constants": {{ catapult_zero | tojson(indent=2)}},
- "potentiometer_offset": 8.54315997763211
+ "potentiometer_offset": {{ 8.54315997763211 - 0.189243355470936 + 0.148884325074452 }}
},
"altitude_constants": {
{% set _ = altitude_zero.update(
@@ -57,20 +57,20 @@
"turret_constants": {
{% set _ = turret_zero.update(
{
- "measured_absolute_position" : 0.0
+ "measured_absolute_position" : 0.143052032770428
}
) %}
"zeroing_constants": {{ turret_zero | tojson(indent=2)}},
- "potentiometer_offset": 0.0
+ "potentiometer_offset": {{ -6.26847263904651 + 0.00657023078932103 }}
},
"extend_constants": {
{% set _ = extend_zero.update(
{
- "measured_absolute_position" : 0.0
+ "measured_absolute_position" : 0.0223997590714573
}
) %}
"zeroing_constants": {{ extend_zero | tojson(indent=2)}},
- "potentiometer_offset": 0.0
+ "potentiometer_offset": {{ -0.2574404033256 + 0.0170793439542 }}
}
},
{% include 'y2024/constants/common.json' %}
diff --git a/y2024/constants/BUILD b/y2024/constants/BUILD
index 1d18a63..b07851e 100644
--- a/y2024/constants/BUILD
+++ b/y2024/constants/BUILD
@@ -117,6 +117,6 @@
constants_json(
name = "test_constants_json",
- src = ":constants_unvalidated.json",
+ src = ":test_constants_unvalidated.json",
out = "test_constants.json",
)
diff --git a/y2024/constants/common.jinja2 b/y2024/constants/common.jinja2
index c2902af..27f28ac 100644
--- a/y2024/constants/common.jinja2
+++ b/y2024/constants/common.jinja2
@@ -6,8 +6,8 @@
{# we do this here so we keep the encoder ratio in plaintext and also keep the math we're using. #}
{% set intake_pivot_encoder_ratio = (15.0 / 24.0) %}
-{% set intake_upper = 2.1 %}
-{% set intake_lower = 0.1 %}
+{% set intake_upper = 1.75 %}
+{% set intake_lower = -0.195 %}
{%
set intake_pivot_zero = {
@@ -33,7 +33,7 @@
%}
{% set climber_encoder_ratio = (16.0 / 60.0) %}
-{% set climber_circumference = 16.0 * 0.25 / 2.0 / pi * 0.0254 %}
+{% set climber_circumference = 16.0 * 0.25 * 0.0254 %}
{%
set climber_zero = {
"average_filter_size": zeroing_sample_size,
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 38d0a76..16acec6 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -23,27 +23,27 @@
}
],
"intake_roller_voltages": {
- "spitting": -12.0,
- "intaking": 12.0
+ "spitting": -4.0,
+ "intaking": 6.0
},
"intake_pivot_set_points": {
- "extended": 0.1,
- "retracted": 2.0
+ "extended": -0.03,
+ "retracted": 1.73
},
"intake_pivot": {
"zeroing_voltage": 3.0,
- "operating_voltage": 12.0,
+ "operating_voltage": 6.0,
"zeroing_profile_params": {
"max_velocity": 0.5,
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 6.0,
- "max_acceleration": 30.0
+ "max_velocity": 0.5,
+ "max_acceleration": 3.0
},
"range": {
- "lower_hard": 0.0,
- "upper_hard": 2.1,
+ "lower_hard": -0.2,
+ "upper_hard": 1.80,
"lower": {{ intake_lower }},
"upper": {{ intake_upper }}
},
@@ -53,40 +53,42 @@
// TODO: (niko) update the stator and supply current limits for the intake
"current_limits": {
// Values in amps
- "intake_pivot_supply_current_limit": 35,
- "intake_pivot_stator_current_limit": 60,
- "intake_roller_supply_current_limit": 35,
- "intake_roller_stator_current_limit": 60,
- "transfer_roller_supply_current_limit": 35,
- "transfer_roller_stator_current_limit": 60,
+ "intake_pivot_supply_current_limit": 5,
+ "intake_pivot_stator_current_limit": 40,
+ "intake_roller_supply_current_limit": 20,
+ "intake_roller_stator_current_limit": 50,
+ "transfer_roller_supply_current_limit": 20,
+ "transfer_roller_stator_current_limit": 50,
"drivetrain_supply_current_limit": 35,
"drivetrain_stator_current_limit": 60,
- "climber_supply_current_limit": 35,
- "climber_stator_current_limit": 300,
- "extend_supply_current_limit": 35,
- "extend_stator_current_limit": 300,
- "extend_roller_supply_current_limit": 35,
- "extend_roller_stator_current_limit": 60,
- "turret_supply_current_limit": 35,
- "turret_stator_current_limit": 60,
- "altitude_supply_current_limit": 35,
+ "climber_supply_current_limit": 30,
+ "climber_stator_current_limit": 100,
+ "extend_supply_current_limit": 20,
+ "extend_stator_current_limit": 100,
+ "extend_roller_supply_current_limit": 60,
+ "extend_roller_stator_current_limit": 200,
+ "turret_supply_current_limit": 20,
+ "turret_stator_current_limit": 40,
+ "altitude_supply_current_limit": 10,
"altitude_stator_current_limit": 60,
+ "catapult_supply_current_limit": 10,
+ "catapult_stator_current_limit": 30,
"retention_roller_stator_current_limit": 5,
"slower_retention_roller_stator_current_limit": 2,
- "retention_roller_supply_current_limit": 60
+ "retention_roller_supply_current_limit": 10
},
"transfer_roller_voltages": {
- "transfer_in": 12.0,
- "transfer_out": -12.0
+ "transfer_in": 6.0,
+ "transfer_out": -4.0
},
"extend_roller_voltages": {
- "scoring": 12.0,
- "reversing": -12.0
+ "scoring": 6.0,
+ "reversing": -4.0
},
"climber_set_points": {
- "full_extend": 0.8,
- "stowed": 0.4,
- "retract": 0.2
+ "full_extend": -0.005,
+ "stowed": -0.35,
+ "retract": -0.478
},
"climber": {
"zeroing_voltage": 3.0,
@@ -96,33 +98,33 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 6.0,
- "max_acceleration": 30.0
+ "max_velocity": 0.05,
+ "max_acceleration": 3.0
},
"range": {
- "lower_hard": 0.1,
- "upper_hard": 2.01,
- "lower": 0.2,
- "upper": 2.0
+ "lower_hard": -0.488,
+ "upper_hard": 0.005,
+ "lower": -0.478,
+ "upper": -0.005
},
"loop": {% include 'y2024/control_loops/superstructure/climber/integral_climber_plant.json' %}
},
"catapult": {
"zeroing_voltage": 3.0,
- "operating_voltage": 12.0,
+ "operating_voltage": 3.0,
"zeroing_profile_params": {
"max_velocity": 1.0,
"max_acceleration": 6.0
},
"default_profile_params":{
- "max_velocity": 6.0,
- "max_acceleration": 30.0
+ "max_velocity": 0.025,
+ "max_acceleration": 0.05
},
"range": {
- "lower_hard": -0.15971,
- "upper_hard": 2.85,
- "lower": -0.020,
- "upper": 2.5
+ "lower_hard": -0.05,
+ "upper_hard": 4.2,
+ "lower": 0.0,
+ "upper": 2.38
},
"loop": {% include 'y2024/control_loops/superstructure/catapult/integral_catapult_plant.json' %}
},
@@ -135,13 +137,13 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 6.0,
- "max_acceleration": 30.0
+ "max_velocity": 0.25,
+ "max_acceleration": 0.25
},
"range": {
- "lower_hard": -0.85,
- "upper_hard": 1.85,
- "lower": -0.400,
+ "lower_hard": -0.01,
+ "upper_hard": 1.66,
+ "lower": 0.0135,
"upper": 1.57
},
"loop": {% include 'y2024/control_loops/superstructure/altitude/integral_altitude_plant.json' %}
@@ -154,14 +156,14 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 6.0,
- "max_acceleration": 30.0
+ "max_velocity": 1.00,
+ "max_acceleration": 1.5
},
"range": {
- "lower_hard": -2.36,
- "upper_hard": 2.36,
- "lower": -2.16,
- "upper": 2.16
+ "lower_hard": -4.8,
+ "upper_hard": 4.8,
+ "lower": -4.7,
+ "upper": 4.7
},
"loop": {% include 'y2024/control_loops/superstructure/turret/integral_turret_plant.json' %}
},
@@ -173,14 +175,14 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 6.0,
- "max_acceleration": 20.0
+ "max_velocity": 0.1,
+ "max_acceleration": 0.3
},
"range": {
- "lower_hard": -0.85,
- "upper_hard": 1.85,
- "lower": -0.400,
- "upper": 1.57
+ "lower_hard": -0.005,
+ "upper_hard": 0.47,
+ "lower": 0.005,
+ "upper": 0.46
},
"loop": {% include 'y2024/control_loops/superstructure/extend/integral_extend_plant.json' %}
},
@@ -208,10 +210,11 @@
"theta": 0.0
}
},
- "altitude_loading_position": 0.0,
- "turret_loading_position": 0.0,
+ "altitude_loading_position": 0.02,
+ "turret_loading_position": 0.58,
"catapult_return_position": 0.0,
- "min_altitude_shooting_angle": 0.3,
+ "min_altitude_shooting_angle": 0.55,
+ "max_altitude_shooting_angle": 0.89,
"retention_roller_voltages": {
"retaining": 1.5,
"spitting": -6.0
@@ -219,7 +222,7 @@
// TODO(Filip): Update the speaker and amp shooter setpoints
"shooter_speaker_set_point": {
"turret_position": 0.0,
- "altitude_position": 0.0,
+ "altitude_position": 0.75,
"shot_velocity": 0.0
},
"shooter_podium_set_point":{
@@ -228,10 +231,10 @@
"shot_velocity": 0.0
},
"extend_set_points": {
- "trap": 0.2,
- "amp": 0.3,
- "catapult": 0.1,
- "retracted": 0.0
+ "trap": 0.46,
+ "amp": 0.2,
+ "catapult": 0.017,
+ "retracted": 0.017
},
- "turret_avoid_extend_collision_position": 1.0
+ "turret_avoid_extend_collision_position": 0.0
}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index 883a945..d2e4ede 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -83,6 +83,8 @@
retention_roller_supply_current_limit:double (id: 18);
retention_roller_stator_current_limit:double (id: 19);
slower_retention_roller_stator_current_limit:double (id: 20);
+ catapult_supply_current_limit:double (id: 21);
+ catapult_stator_current_limit:double (id: 22);
}
table TransferRollerVoltages {
@@ -162,6 +164,7 @@
altitude_loading_position: double (id: 18);
retention_roller_voltages:RetentionRollerVoltages (id: 19);
min_altitude_shooting_angle: double (id: 20);
+ max_altitude_shooting_angle: double (id: 25);
shooter_speaker_set_point: ShooterSetPoint (id: 21);
shooter_podium_set_point: ShooterSetPoint (id: 22);
extend_set_points:ExtendSetPoints (id: 23);
diff --git a/y2024/constants/test_data/calibration_cam-1.json b/y2024/constants/test_data/calibration_cam-1.json
new file mode 100644
index 0000000..2d81347
--- /dev/null
+++ b/y2024/constants/test_data/calibration_cam-1.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "camera_number": 0,
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ 0.0,
+ 1.0,
+ 1.0,
+
+ -1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ -1.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2024/constants/test_data/calibration_cam-2.json b/y2024/constants/test_data/calibration_cam-2.json
new file mode 100644
index 0000000..1128799
--- /dev/null
+++ b/y2024/constants/test_data/calibration_cam-2.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "camera_number": 1,
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 1.0,
+ 0.0,
+ 0.0,
+ 1.0,
+
+ 0.0,
+ 0.0,
+ -1.0,
+ 0.0,
+
+ 0.0,
+ 1.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2024/constants/test_data/calibration_cam-3.json b/y2024/constants/test_data/calibration_cam-3.json
new file mode 100644
index 0000000..01d1ac0
--- /dev/null
+++ b/y2024/constants/test_data/calibration_cam-3.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin2",
+ "camera_number": 0,
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ 1.0,
+ 0.0,
+ 1.0,
+
+ 0.0,
+ 0.0,
+ -1.0,
+ 0.0,
+
+ -1.0,
+ 0.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2024/constants/test_data/calibration_cam-4.json b/y2024/constants/test_data/calibration_cam-4.json
new file mode 100644
index 0000000..3360781
--- /dev/null
+++ b/y2024/constants/test_data/calibration_cam-4.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin2",
+ "camera_number": 1,
+ "team_number": 7971,
+ "intrinsics": [
+ 893.759521,
+ 0,
+ 645.470764,
+ 0,
+ 893.222351,
+ 388.150269,
+ 0,
+ 0,
+ 1
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -1.0,
+ 0.0,
+ 0.0,
+ 1.0,
+
+ 0.0,
+ 0.0,
+ -1.0,
+ 0.0,
+
+ 0.0,
+ -1.0,
+ 0.0,
+ 0.0,
+
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.44902,
+ 0.248409,
+ -0.000537,
+ -0.000112,
+ -0.076989
+ ]
+}
diff --git a/y2024/constants/test_data/test_team.json b/y2024/constants/test_data/test_team.json
index ec59033..b717224 100644
--- a/y2024/constants/test_data/test_team.json
+++ b/y2024/constants/test_data/test_team.json
@@ -8,7 +8,16 @@
{
"cameras": [
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1_cam-24-00.json' %}
+ "calibration": {% include 'y2024/constants/test_data/calibration_cam-1.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/test_data/calibration_cam-2.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/test_data/calibration_cam-3.json' %}
+ },
+ {
+ "calibration": {% include 'y2024/constants/test_data/calibration_cam-4.json' %}
}
],
"robot": {
diff --git a/y2024/control_loops/python/intake_pivot.py b/y2024/control_loops/python/intake_pivot.py
index a699a2f..5e60311 100644
--- a/y2024/control_loops/python/intake_pivot.py
+++ b/y2024/control_loops/python/intake_pivot.py
@@ -22,8 +22,8 @@
motor=control_loop.KrakenFOC(),
G=(16. / 60.) * (18. / 62.) * (18. / 62.) * (15. / 24.),
J=0.25,
- q_pos=0.40,
- q_vel=60.0,
+ q_pos=0.80,
+ q_vel=30.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
kalman_q_voltage=2.0,
diff --git a/y2024/control_loops/superstructure/collision_avoidance.h b/y2024/control_loops/superstructure/collision_avoidance.h
index 8fd14d7..fe36090 100644
--- a/y2024/control_loops/superstructure/collision_avoidance.h
+++ b/y2024/control_loops/superstructure/collision_avoidance.h
@@ -31,12 +31,9 @@
bool operator!=(const Status &s) const { return !(*this == s); }
};
- // Reference angles between which the turret will be careful
- static constexpr double kCollisionZoneTurret = M_PI * 7.0 / 18.0;
-
// For the turret, 0 rad is pointing straight forwards
- static constexpr double kMinCollisionZoneTurret = 0.07;
- static constexpr double kMaxCollisionZoneTurret = 2.1;
+ static constexpr double kMinCollisionZoneTurret = 0.15;
+ static constexpr double kMaxCollisionZoneTurret = 1.15;
// Maximum position of the intake to avoid collisions
static constexpr double kCollisionZoneIntake = 1.6;
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 3216b69..532109e 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -192,7 +192,7 @@
//
// Correct handles resetting our state when disabled.
const bool disabled = catapult_.Correct(nullptr, position->catapult(),
- shooter_goal == nullptr);
+ catapult_output == nullptr);
catapult_.set_enable_profile(true);
// We want a trajectory which accelerates up over the first portion of the
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index ff25bbe..46c7ab1 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -19,6 +19,9 @@
constexpr double kTurretLoadingThreshold = 0.01;
constexpr double kAltitudeLoadingThreshold = 0.01;
+constexpr std::chrono::milliseconds kExtraIntakingTime =
+ std::chrono::milliseconds(500);
+
namespace y2024::control_loops::superstructure {
using ::aos::monotonic_clock;
@@ -61,8 +64,6 @@
const monotonic_clock::time_point timestamp =
event_loop()->context().monotonic_event_time;
- (void)timestamp;
-
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
intake_pivot_.Reset();
@@ -113,6 +114,7 @@
case IntakeGoal::INTAKE:
intake_pivot_position =
robot_constants_->common()->intake_pivot_set_points()->extended();
+ intake_end_time_ = timestamp;
break;
case IntakeGoal::SPIT:
intake_pivot_position =
@@ -168,6 +170,7 @@
// 7. FIRING. The note is being fired, either from the extend or the catapult.
// Switch state back to IDLE when the note is fired.
+ std::optional<bool> turret_ready_for_extend_move;
switch (state_) {
case SuperstructureState::IDLE:
if (unsafe_goal != nullptr &&
@@ -180,7 +183,6 @@
catapult_requested_ = false;
break;
case SuperstructureState::INTAKING:
-
// Switch to LOADED state when the extend beambreak is triggered
// meaning the note is loaded in the extend
if (position->extend_beambreak()) {
@@ -196,6 +198,14 @@
catapult_requested_ = true;
}
+ // If we are no longer requesting INTAKE or we are no longer requesting
+ // an INTAKE goal, wait 0.5 seconds then go back to IDLE.
+ if (!(unsafe_goal != nullptr &&
+ unsafe_goal->intake_goal() == IntakeGoal::INTAKE) &&
+ timestamp > intake_end_time_ + kExtraIntakingTime) {
+ state_ = SuperstructureState::IDLE;
+ }
+
break;
case SuperstructureState::LOADED:
if (!position->extend_beambreak() && !position->catapult_beambreak()) {
@@ -213,13 +223,14 @@
// avoid collision when the extend moves.
if (unsafe_goal->note_goal() == NoteGoal::AMP ||
unsafe_goal->note_goal() == NoteGoal::TRAP) {
- bool turret_ready_for_extend_move =
+ turret_ready_for_extend_move =
PositionNear(shooter_.turret().estimated_position(),
robot_constants_->common()
->turret_avoid_extend_collision_position(),
kTurretLoadingThreshold);
+ transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
- if (turret_ready_for_extend_move) {
+ if (turret_ready_for_extend_move.value()) {
state_ = SuperstructureState::MOVING;
} else {
move_turret_to_standby = true;
@@ -236,6 +247,7 @@
}
break;
case SuperstructureState::MOVING:
+ transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
if (catapult_requested_) {
extend_goal = ExtendStatus::CATAPULT;
@@ -350,6 +362,7 @@
state_ = SuperstructureState::IDLE;
}
} else {
+ move_turret_to_standby = true;
if (unsafe_goal != nullptr &&
unsafe_goal->note_goal() == NoteGoal::AMP) {
extend_roller_status = ExtendRollerStatus::SCORING_IN_AMP;
@@ -434,6 +447,17 @@
double extend_position = 0.0;
+ if (unsafe_goal != nullptr && unsafe_goal->note_goal() == NoteGoal::TRAP) {
+ extend_goal = ExtendStatus::TRAP;
+ move_turret_to_standby = true;
+ }
+
+ // In lieu of having full collision avoidance ready, move the turret out of
+ // the way whenever the extend is raised too much.
+ if (extend_.position() > 0.05) {
+ move_turret_to_standby = true;
+ }
+
// Set the extend position based on the state machine output
switch (extend_goal) {
case ExtendStatus::RETRACTED:
@@ -581,6 +605,11 @@
status_builder.add_extend_status(extend_status);
status_builder.add_extend(extend_status_offset);
status_builder.add_state(state_);
+ status_builder.add_extend_ready_for_transfer(extend_at_retracted);
+ if (turret_ready_for_extend_move) {
+ status_builder.add_turret_ready_for_extend_move(
+ turret_ready_for_extend_move.value());
+ }
(void)status->Send(status_builder.Finish());
}
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 963d8c4..868f11f 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -74,6 +74,9 @@
aos::monotonic_clock::time_point transfer_start_time_ =
aos::monotonic_clock::time_point::min();
+ aos::monotonic_clock::time_point intake_end_time_ =
+ aos::monotonic_clock::time_point::min();
+
AbsoluteEncoderSubsystem intake_pivot_;
PotAndAbsoluteEncoderSubsystem climber_;
diff --git a/y2024/control_loops/superstructure/superstructure_can_position.fbs b/y2024/control_loops/superstructure/superstructure_can_position.fbs
index efe0db2..e809adf 100644
--- a/y2024/control_loops/superstructure/superstructure_can_position.fbs
+++ b/y2024/control_loops/superstructure/superstructure_can_position.fbs
@@ -38,6 +38,8 @@
// CAN Position of the extend roller fancon
extend_roller:frc971.control_loops.CANTalonFX (id: 10);
+ catapult_one:frc971.control_loops.CANTalonFX (id: 11);
+ catapult_two:frc971.control_loops.CANTalonFX (id: 12);
}
root_type CANPosition;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index bafe93f..2850031 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -514,9 +514,9 @@
i++;
RunFor(dt());
superstructure_status_fetcher_.Fetch();
- // 2 Seconds
+ // 10 Seconds
- ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
+ ASSERT_LE(i, 10.0 / ::aos::time::DurationInSeconds(dt()));
// Since there is a delay when sending running, make sure we have a
// status before checking it.
@@ -839,49 +839,6 @@
CheckIfZeroed();
}
-// Tests Climber in multiple scenarios
-TEST_F(SuperstructureTest, ClimberTest) {
- SetEnabled(true);
- WaitUntilZeroed();
-
- superstructure_plant_.climber()->InitializePosition(
- frc971::constants::Range::FromFlatbuffer(
- simulated_robot_constants_->common()->climber()->range())
- .middle());
-
- WaitUntilZeroed();
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
- goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
-
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
-
- RunFor(chrono::seconds(5));
-
- VerifyNearGoal();
-
- WaitUntilZeroed();
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
- goal_builder.add_climber_goal(ClimberGoal::RETRACT);
-
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
-
- RunFor(chrono::seconds(5));
-
- VerifyNearGoal();
-}
-
// Tests intake and transfer in multiple scenarios
TEST_F(SuperstructureTest, IntakeGoal) {
SetEnabled(true);
@@ -967,6 +924,41 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ RunFor(chrono::milliseconds(500));
+
+ // Make sure we're still intaking for 500 ms after we stop giving it an
+ // intaking goal.
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_note_goal(NoteGoal::NONE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::milliseconds(200));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::INTAKING);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_roller(),
+ IntakeRollerStatus::INTAKING);
+
+ // Make sure we stop when loaded
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_note_goal(NoteGoal::NONE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
superstructure_plant_.set_extend_beambreak(true);
RunFor(chrono::seconds(2));
@@ -987,7 +979,7 @@
WaitUntilZeroed();
constexpr double kTurretGoal = 2.0;
- constexpr double kAltitudeGoal = 0.5;
+ constexpr double kAltitudeGoal = 0.55;
set_alliance(aos::Alliance::kRed);
SendDrivetrainStatus(0.0, {0.0, 5.0}, 0.0);
@@ -1026,7 +1018,8 @@
0.01);
EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
- 0.0, 0.01);
+ simulated_robot_constants_->common()->altitude_loading_position(),
+ 0.01);
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::READY);
@@ -1064,7 +1057,8 @@
0.01);
EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
- 0.0, 0.01);
+ simulated_robot_constants_->common()->altitude_loading_position(),
+ 0.01);
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::READY);
@@ -1108,8 +1102,8 @@
EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
ExtendRollerStatus::TRANSFERING_TO_EXTEND);
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 12.0);
- EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 12.0);
+ EXPECT_LT(4.0, superstructure_output_fetcher_->transfer_roller_voltage());
+ EXPECT_LT(4.0, superstructure_output_fetcher_->extend_roller_voltage());
superstructure_plant_.set_extend_beambreak(true);
@@ -1240,7 +1234,9 @@
// Wait until the bot finishes auto-aiming.
WaitUntilNear(kTurretGoal, kAltitudeGoal);
- RunFor(chrono::milliseconds(10));
+ RunFor(chrono::seconds(10));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::FIRING);
@@ -1529,13 +1525,6 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- RunFor(100 * dt());
-
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
- ExtendStatus::MOVING);
-
RunFor(chrono::seconds(5));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
@@ -1572,7 +1561,7 @@
EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
ExtendRollerStatus::SCORING_IN_AMP);
- EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 12.0);
+ EXPECT_LT(4.0, superstructure_output_fetcher_->extend_roller_voltage());
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -1615,7 +1604,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- RunFor(chrono::seconds(5));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
@@ -1629,7 +1618,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- RunFor(chrono::seconds(5));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
@@ -1643,7 +1632,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- RunFor(chrono::seconds(5));
+ RunFor(chrono::seconds(10));
VerifyNearGoal();
diff --git a/y2024/control_loops/superstructure/superstructure_output.fbs b/y2024/control_loops/superstructure/superstructure_output.fbs
index 37796ba..24970d0 100644
--- a/y2024/control_loops/superstructure/superstructure_output.fbs
+++ b/y2024/control_loops/superstructure/superstructure_output.fbs
@@ -2,6 +2,7 @@
table Output {
// Voltage of rollers on intake
+ // Positive means intaking a game piece.
intake_roller_voltage:double (id: 0);
// Voltage of intake pivot
diff --git a/y2024/control_loops/superstructure/superstructure_plotter.ts b/y2024/control_loops/superstructure/superstructure_plotter.ts
index 3498f77..3a2dcaa 100644
--- a/y2024/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2024/control_loops/superstructure/superstructure_plotter.ts
@@ -1,5 +1,5 @@
// Provides a plot for debugging robot state-related issues.
-import {AosPlotter} from '../../../aos/network/www/aos_plotter';
+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';
@@ -7,24 +7,214 @@
const TIME = AosPlotter.TIME;
const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH * 2;
-const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 1;
+
+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 goal = aosPlotter.addMessageSource(
- // '/superstructure', 'y2024.control_loops.superstructure.Goal');
- //const output = aosPlotter.addMessageSource(
- // '/superstructure', 'y2024.control_loops.superstructure.Output');
- //const status = aosPlotter.addMessageSource(
- // '/superstructure', 'y2024.control_loops.superstructure.Status');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.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);
+ }
+}
+
+export function plotClimber(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+ const goal = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Status');
const position = aosPlotter.addMessageSource(
'/superstructure', 'y2024.control_loops.superstructure.Position');
- //const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+ {
+ const goalPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ goalPlot.plot.getAxisLabels().setTitle('Climber Goal');
+ goalPlot.plot.getAxisLabels().setXLabel(TIME);
+ goalPlot.plot.getAxisLabels().setYLabel('[enum]');
+ goalPlot.addMessageLine(goal, ['climber_goal']).setColor(RED);
+ }
- const positionPlot =
- aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- positionPlot.plot.getAxisLabels().setTitle('States');
- positionPlot.plot.getAxisLabels().setXLabel(TIME);
- positionPlot.plot.getAxisLabels().setYLabel('wonky state units');
- positionPlot.plot.setDefaultYRange([-1.0, 2.0]);
+ plotSzsdofSubsystem(
+ 'Climber', aosPlotter, element, position, 'climber', status, 'climber',
+ output, 'climber_voltage');
+}
+
+export function plotIntake(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+ const goal = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Position');
+
+ {
+ const goalPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ goalPlot.plot.getAxisLabels().setTitle('Intake Goal');
+ goalPlot.plot.getAxisLabels().setXLabel(TIME);
+ goalPlot.plot.getAxisLabels().setYLabel('[enum]');
+ goalPlot.addMessageLine(goal, ['intake_goal']).setColor(RED);
+ }
+ {
+ const rollerPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ rollerPlot.plot.getAxisLabels().setTitle('Intake Rollers');
+ rollerPlot.plot.getAxisLabels().setXLabel(TIME);
+ rollerPlot.plot.getAxisLabels().setYLabel('[enum,voltage]');
+ rollerPlot.addMessageLine(status, ['intake_roller']).setColor(RED);
+ rollerPlot.addMessageLine(status, ['transfer_roller']).setColor(BLUE);
+ rollerPlot.addMessageLine(output, ['intake_roller_voltage'])
+ .setColor(RED)
+ .setPointSize(0);
+ rollerPlot.addMessageLine(output, ['transfer_roller_voltage'])
+ .setColor(BLUE)
+ .setPointSize(0);
+ }
+
+ plotSzsdofSubsystem(
+ 'Intake', aosPlotter, element, position, 'intake_pivot', status, 'intake_pivot',
+ output, 'intake_pivot_voltage', false);
+}
+
+export function plotExtend(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+ const goal = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Position');
+
+ {
+ const goalPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ goalPlot.plot.getAxisLabels().setTitle('Extend Goal');
+ goalPlot.plot.getAxisLabels().setXLabel(TIME);
+ goalPlot.plot.getAxisLabels().setYLabel('[enum]');
+ goalPlot.addMessageLine(goal, ['intake_goal']).setColor(RED);
+ }
+ {
+ const rollerPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ rollerPlot.plot.getAxisLabels().setTitle('Extend Rollers');
+ rollerPlot.plot.getAxisLabels().setXLabel(TIME);
+ rollerPlot.plot.getAxisLabels().setYLabel('[enum,voltage]');
+ rollerPlot.addMessageLine(status, ['intake_roller']).setColor(RED);
+ rollerPlot.addMessageLine(status, ['transfer_roller']).setColor(BLUE);
+ rollerPlot.addMessageLine(output, ['intake_roller_voltage'])
+ .setColor(RED)
+ .setPointSize(0);
+ rollerPlot.addMessageLine(output, ['transfer_roller_voltage'])
+ .setColor(BLUE)
+ .setPointSize(0);
+ }
+
+ plotSzsdofSubsystem(
+ 'Extend', aosPlotter, element, position, 'extend', status, 'extend',
+ output, 'extend_voltage');
}
diff --git a/y2024/control_loops/superstructure/superstructure_position.fbs b/y2024/control_loops/superstructure/superstructure_position.fbs
index 4f4b75b..b1facba 100644
--- a/y2024/control_loops/superstructure/superstructure_position.fbs
+++ b/y2024/control_loops/superstructure/superstructure_position.fbs
@@ -5,11 +5,14 @@
table Position {
// Values of the encoder and potentiometer at the intake pivot
- // Zero is extended outwards and level, positive is retracted inward.
+ // Zero is when the lowest extent of the lexan is level with the
+ // bellypan, positive is retracted inward.
intake_pivot:frc971.AbsolutePosition (id: 0);
// Values of the encoder and potentiometer at the turret
- // Zero is facing forward, positive is rotated right.
+ // Zero is facing backwards, positive is rotated counter-clockwise.
+ // I.e., zero is at approximately the loading position for getting
+ // the game piece from the extend into the catapult.
turret:frc971.PotAndAbsolutePosition (id: 1);
// Values of the encoder and potentiometer at the altitude
@@ -17,22 +20,25 @@
altitude:frc971.PotAndAbsolutePosition (id: 2);
// Values of the encoder and potentiometer at the catapult
- // Zero is edge of pivot plate parallel to edge of gusset.
- // Positive is rotated counter-clockwise, to launch game piece.
+ // Zero is when the note is fully seated in the catapult and the catapult
+ // arm is just touching the note. Positive is rotated counter-clockwise, to
+ // launch game piece.
catapult:frc971.PotAndAbsolutePosition (id: 3);
// True means there is a game piece in the transfer.
transfer_beambreak:bool (id: 4);
// Values of the encoder and potentiometer at the climber.
- // Zero is fully retracted, positive is extended upward.
+ // Zero is fully extended, with top of the highest slider aligned with the
+ // caps on the tubes for the climber.
+ // Positive is more extended.
climber:frc971.PotAndAbsolutePosition (id: 5);
// True if there is a game piece in the catapult
catapult_beambreak:bool (id: 6);
// Values of the encoder and potentiometer at the extend motor
- // Zero is fully retracted, positive is extended outward.
+ // Zero is fully retracted, positive is extended upward.
extend:frc971.PotAndAbsolutePosition (id: 7);
// True means there is a game piece in the extend.
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index 7541b7e..e9fff2f 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -6,12 +6,12 @@
enum SuperstructureState : ubyte {
// Before a note has been intaked, the extend should be retracted.
IDLE = 0,
- // Intaking a note and transferring it to the extned through the
+ // Intaking a note and transferring it to the extned through the
// intake, transfer, and extend rollers.
INTAKING = 1,
// The note is in the extend and the extend is not moving.
LOADED = 2,
- // The note is in the extend and the extend is moving towards a goal,
+ // The note is in the extend and the extend is moving towards a goal,
// either the catapult, amp, or trap.
MOVING = 3,
// For Catapult Path, the note is being transferred between the extend and the catapult.
@@ -138,6 +138,13 @@
collided: bool (id: 10);
extend_status:ExtendStatus (id: 11);
+
+ // Indicates that the extend is in position to allow a game piece to
+ // be transfered into it.
+ extend_ready_for_transfer:bool (id: 12);
+
+ // Indicates that the turret is in position to avoid the extend.
+ turret_ready_for_extend_move:bool (id: 13);
}
root_type Status;
diff --git a/y2024/joystick_reader.cc b/y2024/joystick_reader.cc
index 6344127..caa6805 100644
--- a/y2024/joystick_reader.cc
+++ b/y2024/joystick_reader.cc
@@ -36,17 +36,21 @@
namespace superstructure = y2024::control_loops::superstructure;
-// TODO(Xander): add x,y location from physical wiring
-const ButtonLocation kIntake(0, 0);
+// TODO(Xander): add button location from physical wiring
+// Note: Due to use_redundant_joysticks, the AOS_LOG statements
+// for the internal joystick code will give offset joystick numbering.
+const ButtonLocation kIntake(2, 8);
const ButtonLocation kSpit(0, 0);
-const ButtonLocation kCatapultLoad(0, 0);
-const ButtonLocation kAmp(0, 0);
-const ButtonLocation kTrap(0, 0);
+const ButtonLocation kCatapultLoad(1, 7);
+const ButtonLocation kAmp(2, 7);
+const ButtonLocation kFire(2, 6);
+const ButtonLocation kTrap(2, 5);
const ButtonLocation kAutoAim(0, 0);
-const ButtonLocation kAimSpeaker(0, 0);
+const ButtonLocation kAimSpeaker(1, 6);
const ButtonLocation kAimPodium(0, 0);
const ButtonLocation kShoot(0, 0);
-const ButtonLocation kClimb(0, 0);
+const ButtonLocation kRaiseClimber(3, 2);
+const ButtonLocation kRetractClimber(2, 4);
const ButtonLocation kExtraButtonOne(0, 0);
const ButtonLocation kExtraButtonTwo(0, 0);
const ButtonLocation kExtraButtonThree(0, 0);
@@ -72,7 +76,6 @@
void HandleTeleop(
const ::frc971::input::driver_station::Data &data) override {
- (void)data;
superstructure_status_fetcher_.Fetch();
if (!superstructure_status_fetcher_.get()) {
AOS_LOG(ERROR, "Got no superstructure status message.\n");
@@ -87,37 +90,28 @@
// Intake is pressed
superstructure_goal_builder->set_intake_goal(
superstructure::IntakeGoal::INTAKE);
- } else if (data.IsPressed(kSpit)) {
- // If Intake not pressed and spit pressed, spit
+ } else {
superstructure_goal_builder->set_intake_goal(
- superstructure::IntakeGoal::SPIT);
+ superstructure::IntakeGoal::NONE);
}
-
- // Set note goal for the robot. Loading the catapult will always be
- // preferred over scoring in the Amp or Trap.
- if (data.IsPressed(kCatapultLoad)) {
- superstructure_goal_builder->set_note_goal(
- superstructure::NoteGoal::CATAPULT);
- } else if (data.IsPressed(kAmp)) {
+ if (data.IsPressed(kAmp)) {
superstructure_goal_builder->set_note_goal(superstructure::NoteGoal::AMP);
} else if (data.IsPressed(kTrap)) {
superstructure_goal_builder->set_note_goal(
superstructure::NoteGoal::TRAP);
+ } else if (data.IsPressed(kCatapultLoad)) {
+ superstructure_goal_builder->set_note_goal(
+ superstructure::NoteGoal::CATAPULT);
+ } else {
+ superstructure_goal_builder->set_note_goal(
+ superstructure::NoteGoal::NONE);
}
-
- // Firing note when requested
- superstructure_goal_builder->set_fire(data.IsPressed(kShoot));
-
- // Shooter goal contains all speaker-related goals
auto shooter_goal = superstructure_goal_builder->add_shooter_goal();
-
- shooter_goal->set_auto_aim(data.IsPressed(kAimSpeaker));
+ shooter_goal->set_auto_aim(false);
// Updating aiming for shooter goal, only one type of aim should be possible
// at a time, auto-aiming is preferred over the setpoints.
- if (data.IsPressed(kAutoAim)) {
- shooter_goal->set_auto_aim(true);
- } else if (data.IsPressed(kAimSpeaker)) {
+ if (data.IsPressed(kAimSpeaker)) {
auto catapult_goal = shooter_goal->add_catapult_goal();
catapult_goal->set_shot_velocity(robot_constants_->common()
->shooter_speaker_set_point()
@@ -131,29 +125,18 @@
shooter_goal->add_turret_position(), robot_constants_->common()
->shooter_speaker_set_point()
->turret_position());
- } else if (data.IsPressed(kAimPodium)) {
- auto catapult_goal = shooter_goal->add_catapult_goal();
- catapult_goal->set_shot_velocity(robot_constants_->common()
- ->shooter_podium_set_point()
- ->shot_velocity());
- PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
- shooter_goal->add_altitude_position(),
- robot_constants_->common()
- ->shooter_podium_set_point()
- ->altitude_position());
- PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
- shooter_goal->add_turret_position(), robot_constants_->common()
- ->shooter_podium_set_point()
- ->turret_position());
}
+ superstructure_goal_builder->set_fire(data.IsPressed(kFire));
- // Extend climbers if pressed, retract otherwise
- if (data.IsPressed(kClimb)) {
+ if (data.IsPressed(kRaiseClimber)) {
superstructure_goal_builder->set_climber_goal(
superstructure::ClimberGoal::FULL_EXTEND);
- } else {
+ } else if (data.IsPressed(kRetractClimber)) {
superstructure_goal_builder->set_climber_goal(
superstructure::ClimberGoal::RETRACT);
+ } else {
+ superstructure_goal_builder->set_climber_goal(
+ superstructure::ClimberGoal::STOWED);
}
superstructure_goal_builder.CheckOk(superstructure_goal_builder.Send());
diff --git a/y2024/localizer/BUILD b/y2024/localizer/BUILD
new file mode 100644
index 0000000..caec284
--- /dev/null
+++ b/y2024/localizer/BUILD
@@ -0,0 +1,168 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("//tools/build_rules:js.bzl", "ts_project")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+ts_project(
+ name = "localizer_plotter",
+ srcs = ["localizer_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ "//frc971/wpilib:imu_plot_utils",
+ ],
+)
+
+static_flatbuffer(
+ name = "status_fbs",
+ srcs = [
+ "status.fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/imu_reader:imu_failures_fbs",
+ ],
+)
+
+flatbuffer_ts_library(
+ name = "status_ts_fbs",
+ srcs = ["status.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+ "//frc971/imu_reader:imu_failures_ts_fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "visualization_fbs",
+ srcs = [
+ "visualization.fbs",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":status_fbs",
+ ],
+)
+
+flatbuffer_ts_library(
+ name = "visualization_ts_fbs",
+ srcs = ["visualization.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":status_ts_fbs",
+ ],
+)
+
+cc_library(
+ name = "localizer",
+ srcs = ["localizer.cc"],
+ hdrs = ["localizer.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":status_fbs",
+ ":visualization_fbs",
+ "//aos/containers:sized_array",
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_client_fbs",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops/drivetrain:hybrid_ekf",
+ "//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/control_loops/drivetrain/localization:utils",
+ "//frc971/imu_reader:imu_watcher",
+ "//frc971/vision:target_map_fbs",
+ "//frc971/vision:target_map_utils",
+ "//y2024:constants",
+ "//y2024/constants:constants_fbs",
+ ],
+)
+
+cc_test(
+ name = "localizer_test",
+ srcs = ["localizer_test.cc"],
+ data = ["//y2024:aos_config"],
+ deps = [
+ ":localizer",
+ ":status_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/testing:googletest",
+ "//frc971/control_loops/drivetrain:drivetrain_test_lib",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/control_loops/drivetrain/localization:utils",
+ "//y2024/constants:simulated_constants_sender",
+ "//y2024/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+cc_binary(
+ name = "localizer_main",
+ srcs = ["localizer_main.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":localizer",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//y2024/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+ts_project(
+ name = "corrections_plotter",
+ srcs = ["corrections_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":visualization_ts_fbs",
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ ],
+)
+
+cc_binary(
+ name = "localizer_replay",
+ srcs = ["localizer_replay.cc"],
+ data = [
+ "//y2024:aos_config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":localizer",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/events/logging:log_writer",
+ "//aos/util:simulation_logger",
+ "//y2024/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+cc_binary(
+ name = "localizer_logger",
+ srcs = [
+ "localizer_logger.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/events/logging:snappy_encoder",
+ "//aos/logging:log_namer",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/y2024/localizer/corrections_plotter.ts b/y2024/localizer/corrections_plotter.ts
new file mode 100644
index 0000000..dbfbda6
--- /dev/null
+++ b/y2024/localizer/corrections_plotter.ts
@@ -0,0 +1,177 @@
+import {ByteBuffer} from 'flatbuffers';
+import {AosPlotter} from '../../aos/network/www/aos_plotter';
+import {MessageHandler, TimestampedMessage} from '../../aos/network/www/aos_plotter';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from '../../aos/network/www/colors';
+import {Connection} from '../../aos/network/www/proxy';
+import {Table} from '../../aos/network/www/reflection';
+import {Schema} from 'flatbuffers_reflection/reflection_generated';
+import {Visualization, TargetEstimateDebug} from './visualization_generated';
+
+
+const TIME = AosPlotter.TIME;
+// magenta, yellow, cyan, black
+const PI_COLORS = [[255, 0, 255], [255, 255, 0], [0, 255, 255], [0, 0, 0]];
+
+class VisionMessageHandler extends MessageHandler {
+ constructor(private readonly schema: Schema) {
+ super(schema);
+ }
+
+ private readScalar(table: Table, fieldName: string): number|BigInt|null {
+ return this.parser.readScalar(table, fieldName);
+ }
+
+ addMessage(data: Uint8Array, time: number): void {
+ const message = Visualization.getRootAsVisualization(new ByteBuffer(data));
+ for (let ii = 0; ii < message.targetsLength(); ++ii) {
+ const target = message.targets(ii);
+ const time = Number(target.imageMonotonicTimestampNs()) * 1e-9;
+ if (time == 0) {
+ console.log('Dropping message without populated time?');
+ continue;
+ }
+ const table = Table.getNamedTable(
+ target.bb, this.schema, 'y2024.localizer.TargetEstimateDebug', target.bb_pos);
+ this.messages.push(new TimestampedMessage(table, time));
+ }
+ }
+}
+
+export function plotVision(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+
+ const targets = [];
+ const targetLabels = [];
+ for (const orin of ['orin1', 'orin2']) {
+ for (const camera of ['camera0', 'camera1']) {
+ targetLabels.push(orin + ' ' + camera);
+ targets.push(aosPlotter.addRawMessageSource(
+ '/' + orin + '/' + camera, 'y2024.localizer.Visualization',
+ new VisionMessageHandler(
+ conn.getSchema('y2024.localizer.Visualization'))));
+ }
+ }
+ const localizerStatus = aosPlotter.addMessageSource(
+ '/localizer', 'y2024.localizer.Status');
+ const localizerOutput = aosPlotter.addMessageSource(
+ '/localizer', 'frc971.controls.LocalizerOutput');
+
+ const statsPlot = aosPlotter.addPlot(element);
+ statsPlot.plot.getAxisLabels().setTitle('Statistics');
+ statsPlot.plot.getAxisLabels().setXLabel(TIME);
+ statsPlot.plot.getAxisLabels().setYLabel('[bool, enum]');
+
+ statsPlot
+ .addMessageLine(localizerStatus, ['statistics[]', 'total_accepted'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+ statsPlot
+ .addMessageLine(localizerStatus, ['statistics[]', 'total_candidates'])
+ .setDrawLine(false)
+ .setColor(RED);
+
+ const rejectionPlot = aosPlotter.addPlot(element);
+ rejectionPlot.plot.getAxisLabels().setTitle('Rejection Reasons');
+ rejectionPlot.plot.getAxisLabels().setXLabel(TIME);
+ rejectionPlot.plot.getAxisLabels().setYLabel('[bool, enum]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ rejectionPlot.addMessageLine(targets[ii], ['rejection_reason'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel(targetLabels[ii]);
+ }
+
+ const xPlot = aosPlotter.addPlot(element);
+ xPlot.plot.getAxisLabels().setTitle('X Position');
+ xPlot.plot.getAxisLabels().setXLabel(TIME);
+ xPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ xPlot.addMessageLine(targets[ii], ['implied_robot_x'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
+ xPlot.addMessageLine(localizerOutput, ['x'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+
+ const correctionXPlot = aosPlotter.addPlot(element);
+ correctionXPlot.plot.getAxisLabels().setTitle('X Corrections');
+ correctionXPlot.plot.getAxisLabels().setXLabel(TIME);
+ correctionXPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ correctionXPlot.addMessageLine(targets[ii], ['correction_x'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
+
+ const yPlot = aosPlotter.addPlot(element);
+ yPlot.plot.getAxisLabels().setTitle('Y Position');
+ yPlot.plot.getAxisLabels().setXLabel(TIME);
+ yPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ yPlot.addMessageLine(targets[ii], ['implied_robot_y'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
+ yPlot.addMessageLine(localizerOutput, ['y'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+
+ const correctionYPlot = aosPlotter.addPlot(element);
+ correctionYPlot.plot.getAxisLabels().setTitle('Y Corrections');
+ correctionYPlot.plot.getAxisLabels().setXLabel(TIME);
+ correctionYPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ correctionYPlot.addMessageLine(targets[ii], ['correction_y'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
+
+ const thetaPlot = aosPlotter.addPlot(element);
+ thetaPlot.plot.getAxisLabels().setTitle('Yaw');
+ thetaPlot.plot.getAxisLabels().setXLabel(TIME);
+ thetaPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ thetaPlot.addMessageLine(targets[ii], ['implied_robot_theta'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
+ thetaPlot.addMessageLine(localizerOutput, ['theta'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+
+ const aprilTagPlot = aosPlotter.addPlot(element);
+ aprilTagPlot.plot.getAxisLabels().setTitle('April Tag IDs');
+ aprilTagPlot.plot.getAxisLabels().setXLabel(TIME);
+ aprilTagPlot.plot.getAxisLabels().setYLabel('[id]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ aprilTagPlot.addMessageLine(targets[ii], ['april_tag'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
+
+ const imageAgePlot = aosPlotter.addPlot(element);
+ imageAgePlot.plot.getAxisLabels().setTitle('Image Age');
+ imageAgePlot.plot.getAxisLabels().setXLabel(TIME);
+ imageAgePlot.plot.getAxisLabels().setYLabel('[sec]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ imageAgePlot.addMessageLine(targets[ii], ['image_age_sec'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
+}
diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
new file mode 100644
index 0000000..34c0bc7
--- /dev/null
+++ b/y2024/localizer/localizer.cc
@@ -0,0 +1,605 @@
+#include "y2024/localizer/localizer.h"
+
+#include "gflags/gflags.h"
+
+#include "aos/containers/sized_array.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/vision/target_map_utils.h"
+#include "y2024/constants.h"
+
+DEFINE_double(max_pose_error, 1e-6,
+ "Throw out target poses with a higher pose error than this");
+DEFINE_double(
+ max_pose_error_ratio, 0.4,
+ "Throw out target poses with a higher pose error ratio than this");
+DEFINE_double(distortion_noise_scalar, 1.0,
+ "Scale the target pose distortion factor by this when computing "
+ "the noise.");
+DEFINE_double(
+ max_implied_yaw_error, 3.0,
+ "Reject target poses that imply a robot yaw of more than this many degrees "
+ "off from our estimate.");
+DEFINE_double(
+ max_implied_teleop_yaw_error, 30.0,
+ "Reject target poses that imply a robot yaw of more than this many degrees "
+ "off from our estimate.");
+DEFINE_double(max_distance_to_target, 5.0,
+ "Reject target poses that have a 3d distance of more than this "
+ "many meters.");
+DEFINE_double(max_auto_image_robot_speed, 2.0,
+ "Reject target poses when the robot is travelling faster than "
+ "this speed in auto.");
+
+namespace y2024::localizer {
+namespace {
+constexpr std::array<std::string_view, Localizer::kNumCameras>
+ kDetectionChannels{"/orin1/camera0", "/orin1/camera1", "/orin2/camera0",
+ "/orin2/camera1"};
+
+size_t CameraIndexForName(std::string_view name) {
+ for (size_t index = 0; index < kDetectionChannels.size(); ++index) {
+ if (name == kDetectionChannels.at(index)) {
+ return index;
+ }
+ }
+ LOG(FATAL) << "No camera channel named " << name;
+}
+
+std::map<uint64_t, Localizer::Transform> GetTargetLocations(
+ const Constants &constants) {
+ CHECK(constants.has_common());
+ CHECK(constants.common()->has_target_map());
+ CHECK(constants.common()->target_map()->has_target_poses());
+ std::map<uint64_t, Localizer::Transform> transforms;
+ for (const frc971::vision::TargetPoseFbs *target :
+ *constants.common()->target_map()->target_poses()) {
+ CHECK(target->has_id());
+ CHECK(target->has_position());
+ CHECK(target->has_orientation());
+ CHECK_EQ(0u, transforms.count(target->id()));
+ transforms[target->id()] = PoseToTransform(target);
+ }
+ return transforms;
+}
+} // namespace
+
+std::array<Localizer::CameraState, Localizer::kNumCameras>
+Localizer::MakeCameras(const Constants &constants, aos::EventLoop *event_loop) {
+ CHECK(constants.has_cameras());
+ std::array<Localizer::CameraState, Localizer::kNumCameras> cameras;
+ for (const CameraConfiguration *camera : *constants.cameras()) {
+ CHECK(camera->has_calibration());
+ const frc971::vision::calibration::CameraCalibration *calibration =
+ camera->calibration();
+ CHECK(!calibration->has_turret_extrinsics())
+ << "The 2024 robot does not have cameras on a turret.";
+ CHECK(calibration->has_node_name());
+ const std::string channel_name =
+ absl::StrFormat("/%s/camera%d", calibration->node_name()->string_view(),
+ calibration->camera_number());
+ const size_t index = CameraIndexForName(channel_name);
+ // We default-construct the extrinsics matrix to all-zeros; use that to
+ // sanity-check whether we have populated the matrix yet or not.
+ CHECK(cameras.at(index).extrinsics.norm() == 0)
+ << "Got multiple calibrations for "
+ << calibration->node_name()->string_view();
+ CHECK(calibration->has_fixed_extrinsics());
+ cameras.at(index).extrinsics =
+ frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *calibration->fixed_extrinsics());
+ cameras.at(index).debug_sender =
+ event_loop->MakeSender<VisualizationStatic>(channel_name);
+ }
+ for (const CameraState &camera : cameras) {
+ CHECK(camera.extrinsics.norm() != 0) << "Missing a camera calibration.";
+ }
+ return cameras;
+}
+
+Localizer::Localizer(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ constants_fetcher_(event_loop),
+ dt_config_(
+ frc971::control_loops::drivetrain::DrivetrainConfig<double>::
+ FromFlatbuffer(*CHECK_NOTNULL(
+ constants_fetcher_.constants().common()->drivetrain()))),
+ cameras_(MakeCameras(constants_fetcher_.constants(), event_loop)),
+ target_poses_(GetTargetLocations(constants_fetcher_.constants())),
+ down_estimator_(dt_config_),
+ ekf_(dt_config_),
+ observations_(&ekf_),
+ imu_watcher_(event_loop, dt_config_,
+ y2024::constants::Values::DrivetrainEncoderToMeters(1),
+ std::bind(&Localizer::HandleImu, this, std::placeholders::_1,
+ std::placeholders::_2, std::placeholders::_3,
+ std::placeholders::_4, std::placeholders::_5),
+ frc971::controls::ImuWatcher::TimestampSource::kPi),
+ utils_(event_loop),
+ status_sender_(event_loop->MakeSender<Status>("/localizer")),
+ output_sender_(event_loop->MakeSender<frc971::controls::LocalizerOutput>(
+ "/localizer")),
+ server_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/aos")),
+ client_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ClientStatistics>(
+ "/aos")) {
+ if (dt_config_.is_simulated) {
+ down_estimator_.assume_perfect_gravity();
+ }
+
+ for (size_t camera_index = 0; camera_index < kNumCameras; ++camera_index) {
+ const std::string_view channel_name = kDetectionChannels.at(camera_index);
+ const aos::Channel *const channel = CHECK_NOTNULL(
+ event_loop->GetChannel<frc971::vision::TargetMap>(channel_name));
+ event_loop->MakeWatcher(
+ channel_name, [this, channel,
+ camera_index](const frc971::vision::TargetMap &targets) {
+ CHECK(targets.has_target_poses());
+ CHECK(targets.has_monotonic_timestamp_ns());
+ const std::optional<aos::monotonic_clock::duration> clock_offset =
+ utils_.ClockOffset(channel->source_node()->string_view());
+ if (!clock_offset.has_value()) {
+ VLOG(1) << "Rejecting image due to disconnected message bridge at "
+ << event_loop_->monotonic_now();
+ cameras_.at(camera_index)
+ .rejection_counter.IncrementError(
+ RejectionReason::MESSAGE_BRIDGE_DISCONNECTED);
+ return;
+ }
+ const aos::monotonic_clock::time_point orin_capture_time(
+ std::chrono::nanoseconds(targets.monotonic_timestamp_ns()) -
+ clock_offset.value());
+ if (orin_capture_time > event_loop_->context().monotonic_event_time) {
+ VLOG(1) << "Rejecting image due to being from future at "
+ << event_loop_->monotonic_now() << " with timestamp of "
+ << orin_capture_time << " and event time pf "
+ << event_loop_->context().monotonic_event_time;
+ cameras_.at(camera_index)
+ .rejection_counter.IncrementError(
+ RejectionReason::IMAGE_FROM_FUTURE);
+ return;
+ }
+ auto debug_builder =
+ cameras_.at(camera_index).debug_sender.MakeStaticBuilder();
+ auto target_debug_list = debug_builder->add_targets();
+ // The static_length should already be 20.
+ CHECK(target_debug_list->reserve(20));
+ for (const frc971::vision::TargetPoseFbs *target :
+ *targets.target_poses()) {
+ VLOG(1) << "Handling target from " << camera_index;
+ HandleTarget(camera_index, orin_capture_time, *target,
+ target_debug_list->emplace_back());
+ }
+ StatisticsForCamera(cameras_.at(camera_index),
+ debug_builder->add_statistics());
+ debug_builder.CheckOk(debug_builder.Send());
+ SendStatus();
+ });
+ }
+
+ event_loop_->AddPhasedLoop([this](int) { SendOutput(); },
+ std::chrono::milliseconds(20));
+
+ event_loop_->MakeWatcher(
+ "/drivetrain",
+ [this](
+ const frc971::control_loops::drivetrain::LocalizerControl &control) {
+ // This is triggered whenever we need to force the X/Y/(maybe theta)
+ // position of the robot to a particular point---e.g., during pre-match
+ // setup, or when commanded by a button on the driverstation.
+
+ // For some forms of reset, we choose to keep our current yaw estimate
+ // rather than overriding it from the control message.
+ const double theta = control.keep_current_theta()
+ ? ekf_.X_hat(StateIdx::kTheta)
+ : control.theta();
+ // Encoder values need to be reset based on the current values to ensure
+ // that we don't get weird corrections on the next encoder update.
+ const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
+ const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
+ ekf_.ResetInitialState(
+ t_,
+ (HybridEkf::State() << control.x(), control.y(), theta,
+ left_encoder, 0, right_encoder, 0, 0, 0, 0, 0, 0)
+ .finished(),
+ ekf_.P());
+ });
+
+ ekf_.set_ignore_accel(true);
+ // Priority should be lower than the imu reading process, but non-zero.
+ event_loop->SetRuntimeRealtimePriority(10);
+ event_loop->OnRun([this, event_loop]() {
+ ekf_.ResetInitialState(event_loop->monotonic_now(),
+ HybridEkf::State::Zero(), ekf_.P());
+ });
+}
+
+void Localizer::HandleImu(aos::monotonic_clock::time_point /*sample_time_pico*/,
+ aos::monotonic_clock::time_point sample_time_orin,
+ std::optional<Eigen::Vector2d> /*encoders*/,
+ Eigen::Vector3d gyro, Eigen::Vector3d accel) {
+ std::optional<Eigen::Vector2d> encoders = utils_.Encoders(sample_time_orin);
+ last_encoder_readings_ = encoders;
+ // Ignore invalid readings; the HybridEkf will handle it reasonably.
+ if (!encoders.has_value()) {
+ return;
+ }
+ if (t_ == aos::monotonic_clock::min_time) {
+ t_ = sample_time_orin;
+ }
+ if (t_ + 10 * frc971::controls::ImuWatcher::kNominalDt < sample_time_orin) {
+ t_ = sample_time_orin;
+ ++clock_resets_;
+ }
+ const aos::monotonic_clock::duration dt = sample_time_orin - t_;
+ t_ = sample_time_orin;
+ // We don't actually use the down estimator currently, but it's really
+ // convenient for debugging.
+ down_estimator_.Predict(gyro, accel, dt);
+ const double yaw_rate = (dt_config_.imu_transform * gyro)(2);
+ ekf_.UpdateEncodersAndGyro(encoders.value()(0), encoders.value()(1), yaw_rate,
+ utils_.VoltageOrZero(sample_time_orin), accel, t_);
+ SendStatus();
+}
+
+void Localizer::RejectImage(int camera_index, RejectionReason reason,
+ TargetEstimateDebugStatic *builder) {
+ if (builder != nullptr) {
+ builder->set_accepted(false);
+ builder->set_rejection_reason(reason);
+ }
+ cameras_.at(camera_index).rejection_counter.IncrementError(reason);
+}
+
+// Only use april tags present in the target map; this method has also been used
+// (in the past) for ignoring april tags that tend to produce problematic
+// readings.
+bool Localizer::UseAprilTag(uint64_t target_id) {
+ return target_poses_.count(target_id) != 0;
+}
+
+namespace {
+// Converts a camera transformation matrix from treating the +Z axis from
+// pointing straight out the lens to having the +X pointing straight out the
+// lens, with +Z going "up" (i.e., -Y in the normal convention) and +Y going
+// leftwards (i.e., -X in the normal convention).
+Localizer::Transform ZToXCamera(const Localizer::Transform &transform) {
+ return transform *
+ Eigen::Matrix4d{
+ {0, -1, 0, 0}, {0, 0, -1, 0}, {1, 0, 0, 0}, {0, 0, 0, 1}};
+}
+} // namespace
+
+void Localizer::HandleTarget(
+ int camera_index, const aos::monotonic_clock::time_point capture_time,
+ const frc971::vision::TargetPoseFbs &target,
+ TargetEstimateDebugStatic *debug_builder) {
+ ++total_candidate_targets_;
+ ++cameras_.at(camera_index).total_candidate_targets;
+ const uint64_t target_id = target.id();
+
+ if (debug_builder == nullptr) {
+ AOS_LOG(ERROR, "Dropped message from debug vector.");
+ } else {
+ debug_builder->set_camera(camera_index);
+ debug_builder->set_image_age_sec(aos::time::DurationInSeconds(
+ event_loop_->monotonic_now() - capture_time));
+ debug_builder->set_image_monotonic_timestamp_ns(
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ capture_time.time_since_epoch())
+ .count());
+ debug_builder->set_april_tag(target_id);
+ }
+ VLOG(2) << aos::FlatbufferToJson(&target);
+ if (!UseAprilTag(target_id)) {
+ VLOG(1) << "Rejecting target due to invalid ID " << target_id;
+ RejectImage(camera_index, RejectionReason::NO_SUCH_TARGET, debug_builder);
+ return;
+ }
+
+ const Transform &H_field_target = target_poses_.at(target_id);
+ const Transform &H_robot_camera = cameras_.at(camera_index).extrinsics;
+
+ const Transform H_camera_target = PoseToTransform(&target);
+
+ // In order to do the EKF correction, we determine the expected state based
+ // on the state at the time the image was captured; however, we insert the
+ // correction update itself at the current time. This is technically not
+ // quite correct, but saves substantial CPU usage & code complexity by
+ // making it so that we don't have to constantly rewind the entire EKF
+ // history.
+ const std::optional<State> state_at_capture =
+ ekf_.LastStateBeforeTime(capture_time);
+
+ if (!state_at_capture.has_value()) {
+ VLOG(1) << "Rejecting image due to being too old.";
+ return RejectImage(camera_index, RejectionReason::IMAGE_TOO_OLD,
+ debug_builder);
+ } else if (target.pose_error() > FLAGS_max_pose_error) {
+ VLOG(1) << "Rejecting target due to high pose error "
+ << target.pose_error();
+ return RejectImage(camera_index, RejectionReason::HIGH_POSE_ERROR,
+ debug_builder);
+ } else if (target.pose_error_ratio() > FLAGS_max_pose_error_ratio) {
+ VLOG(1) << "Rejecting target due to high pose error ratio "
+ << target.pose_error_ratio();
+ return RejectImage(camera_index, RejectionReason::HIGH_POSE_ERROR_RATIO,
+ debug_builder);
+ }
+
+ Corrector corrector(state_at_capture.value(), H_field_target, H_robot_camera,
+ H_camera_target);
+ const double distance_to_target = corrector.observed()(Corrector::kDistance);
+
+ // Heading, distance, skew at 1 meter.
+ Eigen::Matrix<double, 3, 1> noises(0.01, 0.05, 0.05);
+ const double distance_noise_scalar = std::pow(distance_to_target, 2.0);
+ noises(Corrector::kDistance) *= distance_noise_scalar;
+ noises(Corrector::kSkew) *= distance_noise_scalar;
+ // TODO(james): This is leftover from last year; figure out if we want it.
+ // Scale noise by the distortion factor for this detection
+ noises *= (1.0 + FLAGS_distortion_noise_scalar * target.distortion_factor());
+
+ Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
+ R.diagonal() = noises.cwiseAbs2();
+ if (debug_builder != nullptr) {
+ const Eigen::Vector3d camera_position =
+ corrector.observed_camera_pose().abs_pos();
+ debug_builder->set_camera_x(camera_position.x());
+ debug_builder->set_camera_y(camera_position.y());
+ debug_builder->set_camera_theta(
+ corrector.observed_camera_pose().abs_theta());
+ // Calculate the camera-to-robot transformation matrix ignoring the
+ // pitch/roll of the camera.
+ const Transform H_camera_robot_stripped =
+ frc971::control_loops::Pose(ZToXCamera(H_robot_camera))
+ .AsTransformationMatrix()
+ .inverse();
+ const frc971::control_loops::Pose measured_pose(
+ corrector.observed_camera_pose().AsTransformationMatrix() *
+ H_camera_robot_stripped);
+ debug_builder->set_implied_robot_x(measured_pose.rel_pos().x());
+ debug_builder->set_implied_robot_y(measured_pose.rel_pos().y());
+ debug_builder->set_implied_robot_theta(measured_pose.rel_theta());
+
+ Corrector::PopulateMeasurement(corrector.expected(),
+ debug_builder->add_expected_observation());
+ Corrector::PopulateMeasurement(corrector.observed(),
+ debug_builder->add_actual_observation());
+ Corrector::PopulateMeasurement(noises, debug_builder->add_modeled_noise());
+ }
+
+ const double camera_yaw_error =
+ aos::math::NormalizeAngle(corrector.expected_camera_pose().abs_theta() -
+ corrector.observed_camera_pose().abs_theta());
+ constexpr double kDegToRad = M_PI / 180.0;
+
+ const double robot_speed =
+ (state_at_capture.value()(StateIdx::kLeftVelocity) +
+ state_at_capture.value()(StateIdx::kRightVelocity)) /
+ 2.0;
+ const double yaw_threshold =
+ (utils_.MaybeInAutonomous() ? FLAGS_max_implied_yaw_error
+ : FLAGS_max_implied_teleop_yaw_error) *
+ kDegToRad;
+
+ if (utils_.MaybeInAutonomous() &&
+ (std::abs(robot_speed) > FLAGS_max_auto_image_robot_speed)) {
+ return RejectImage(camera_index, RejectionReason::ROBOT_TOO_FAST,
+ debug_builder);
+ } else if (std::abs(camera_yaw_error) > yaw_threshold) {
+ return RejectImage(camera_index, RejectionReason::HIGH_IMPLIED_YAW_ERROR,
+ debug_builder);
+ } else if (distance_to_target > FLAGS_max_distance_to_target) {
+ return RejectImage(camera_index, RejectionReason::HIGH_DISTANCE_TO_TARGET,
+ debug_builder);
+ }
+
+ const Input U = ekf_.MostRecentInput();
+ VLOG(1) << "previous state " << ekf_.X_hat().topRows<3>().transpose();
+ const State prior_state = ekf_.X_hat();
+ // For the correction step, instead of passing in the measurement directly,
+ // we pass in (0, 0, 0) as the measurement and then for the expected
+ // measurement (Zhat) we calculate the error between the pose implied by
+ // the camera measurement and the current estimate of the
+ // pose. This doesn't affect any of the math, it just makes the code a bit
+ // more convenient to write given the Correct() interface we already have.
+ observations_.CorrectKnownH(
+ Eigen::Vector3d::Zero(), &U,
+ Corrector(state_at_capture.value(), H_field_target, H_robot_camera,
+ H_camera_target),
+ R, t_);
+ ++total_accepted_targets_;
+ ++cameras_.at(camera_index).total_accepted_targets;
+ VLOG(1) << "new state " << ekf_.X_hat().topRows<3>().transpose();
+ if (debug_builder != nullptr) {
+ debug_builder->set_correction_x(ekf_.X_hat()(StateIdx::kX) -
+ prior_state(StateIdx::kX));
+ debug_builder->set_correction_y(ekf_.X_hat()(StateIdx::kY) -
+ prior_state(StateIdx::kY));
+ debug_builder->set_correction_theta(ekf_.X_hat()(StateIdx::kTheta) -
+ prior_state(StateIdx::kTheta));
+ debug_builder->set_accepted(true);
+ }
+}
+
+void Localizer::SendOutput() {
+ auto builder = output_sender_.MakeBuilder();
+ frc971::controls::LocalizerOutput::Builder output_builder =
+ builder.MakeBuilder<frc971::controls::LocalizerOutput>();
+ output_builder.add_monotonic_timestamp_ns(
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ event_loop_->context().monotonic_event_time.time_since_epoch())
+ .count());
+ output_builder.add_x(ekf_.X_hat(StateIdx::kX));
+ output_builder.add_y(ekf_.X_hat(StateIdx::kY));
+ output_builder.add_theta(ekf_.X_hat(StateIdx::kTheta));
+ output_builder.add_zeroed(imu_watcher_.zeroer().Zeroed());
+ output_builder.add_image_accepted_count(total_accepted_targets_);
+ const Eigen::Quaterniond &orientation =
+ Eigen::AngleAxis<double>(ekf_.X_hat(StateIdx::kTheta),
+ Eigen::Vector3d::UnitZ()) *
+ down_estimator_.X_hat();
+ frc971::controls::Quaternion quaternion;
+ quaternion.mutate_x(orientation.x());
+ quaternion.mutate_y(orientation.y());
+ quaternion.mutate_z(orientation.z());
+ quaternion.mutate_w(orientation.w());
+ output_builder.add_orientation(&quaternion);
+ server_statistics_fetcher_.Fetch();
+ client_statistics_fetcher_.Fetch();
+
+ bool orins_connected = true;
+
+ if (server_statistics_fetcher_.get()) {
+ for (const auto *orin_server_status :
+ *server_statistics_fetcher_->connections()) {
+ if (orin_server_status->state() ==
+ aos::message_bridge::State::DISCONNECTED) {
+ orins_connected = false;
+ }
+ }
+ }
+
+ if (client_statistics_fetcher_.get()) {
+ for (const auto *pi_client_status :
+ *client_statistics_fetcher_->connections()) {
+ if (pi_client_status->state() ==
+ aos::message_bridge::State::DISCONNECTED) {
+ orins_connected = false;
+ }
+ }
+ }
+
+ // The output message is year-agnostic, and retains "pi" naming for histrocial
+ // reasons.
+ output_builder.add_all_pis_connected(orins_connected);
+ builder.CheckOk(builder.Send(output_builder.Finish()));
+}
+
+flatbuffers::Offset<frc971::control_loops::drivetrain::LocalizerState>
+Localizer::PopulateState(const State &X_hat,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ frc971::control_loops::drivetrain::LocalizerState::Builder builder(*fbb);
+ builder.add_x(X_hat(StateIdx::kX));
+ builder.add_y(X_hat(StateIdx::kY));
+ builder.add_theta(X_hat(StateIdx::kTheta));
+ builder.add_left_velocity(X_hat(StateIdx::kLeftVelocity));
+ builder.add_right_velocity(X_hat(StateIdx::kRightVelocity));
+ builder.add_left_encoder(X_hat(StateIdx::kLeftEncoder));
+ builder.add_right_encoder(X_hat(StateIdx::kRightEncoder));
+ builder.add_left_voltage_error(X_hat(StateIdx::kLeftVoltageError));
+ builder.add_right_voltage_error(X_hat(StateIdx::kRightVoltageError));
+ builder.add_angular_error(X_hat(StateIdx::kAngularError));
+ builder.add_longitudinal_velocity_offset(
+ X_hat(StateIdx::kLongitudinalVelocityOffset));
+ builder.add_lateral_velocity(X_hat(StateIdx::kLateralVelocity));
+ return builder.Finish();
+}
+
+flatbuffers::Offset<ImuStatus> Localizer::PopulateImu(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ const auto zeroer_offset = imu_watcher_.zeroer().PopulateStatus(fbb);
+ const auto failures_offset = imu_watcher_.PopulateImuFailures(fbb);
+ ImuStatus::Builder builder(*fbb);
+ builder.add_zeroed(imu_watcher_.zeroer().Zeroed());
+ builder.add_faulted_zero(imu_watcher_.zeroer().Faulted());
+ builder.add_zeroing(zeroer_offset);
+ if (imu_watcher_.pico_offset().has_value()) {
+ builder.add_board_offset_ns(imu_watcher_.pico_offset().value().count());
+ builder.add_board_offset_error_ns(imu_watcher_.pico_offset_error().count());
+ }
+ if (last_encoder_readings_.has_value()) {
+ builder.add_left_encoder(last_encoder_readings_.value()(0));
+ builder.add_right_encoder(last_encoder_readings_.value()(1));
+ }
+ builder.add_imu_failures(failures_offset);
+ return builder.Finish();
+}
+
+flatbuffers::Offset<CumulativeStatistics> Localizer::StatisticsForCamera(
+ const CameraState &camera, flatbuffers::FlatBufferBuilder *fbb) {
+ const auto counts_offset = camera.rejection_counter.PopulateCounts(fbb);
+ CumulativeStatistics::Builder stats_builder(*fbb);
+ stats_builder.add_total_accepted(camera.total_accepted_targets);
+ stats_builder.add_total_candidates(camera.total_candidate_targets);
+ stats_builder.add_rejection_reasons(counts_offset);
+ return stats_builder.Finish();
+}
+
+void Localizer::StatisticsForCamera(const CameraState &camera,
+ CumulativeStatisticsStatic *builder) {
+ camera.rejection_counter.PopulateCountsStaticFbs(
+ builder->add_rejection_reasons());
+ builder->set_total_accepted(camera.total_accepted_targets);
+ builder->set_total_candidates(camera.total_candidate_targets);
+}
+
+void Localizer::SendStatus() {
+ auto builder = status_sender_.MakeBuilder();
+ std::array<flatbuffers::Offset<CumulativeStatistics>, kNumCameras>
+ stats_offsets;
+ for (size_t ii = 0; ii < kNumCameras; ++ii) {
+ stats_offsets.at(ii) = StatisticsForCamera(cameras_.at(ii), builder.fbb());
+ }
+ auto stats_offset =
+ builder.fbb()->CreateVector(stats_offsets.data(), stats_offsets.size());
+ auto down_estimator_offset =
+ down_estimator_.PopulateStatus(builder.fbb(), t_);
+ auto imu_offset = PopulateImu(builder.fbb());
+ auto state_offset = PopulateState(ekf_.X_hat(), builder.fbb());
+ Status::Builder status_builder = builder.MakeBuilder<Status>();
+ status_builder.add_state(state_offset);
+ status_builder.add_down_estimator(down_estimator_offset);
+ status_builder.add_imu(imu_offset);
+ status_builder.add_statistics(stats_offset);
+ builder.CheckOk(builder.Send(status_builder.Finish()));
+}
+
+Eigen::Vector3d Localizer::Corrector::HeadingDistanceSkew(
+ const Pose &relative_pose) {
+ const double heading = relative_pose.heading();
+ const double distance = relative_pose.xy_norm();
+ const double skew =
+ ::aos::math::NormalizeAngle(relative_pose.rel_theta() - heading);
+ return {heading, distance, skew};
+}
+
+Localizer::Corrector Localizer::Corrector::CalculateHeadingDistanceSkewH(
+ const State &state_at_capture, const Transform &H_field_target,
+ const Transform &H_robot_camera, const Transform &H_camera_target) {
+ const Transform H_field_camera = H_field_target * H_camera_target.inverse();
+ const Pose expected_robot_pose(
+ {state_at_capture(StateIdx::kX), state_at_capture(StateIdx::kY), 0.0},
+ state_at_capture(StateIdx::kTheta));
+ // Observed position on the field, reduced to just the 2-D pose.
+ const Pose observed_camera(ZToXCamera(H_field_camera));
+ const Pose expected_camera(expected_robot_pose.AsTransformationMatrix() *
+ ZToXCamera(H_robot_camera));
+ const Pose nominal_target(ZToXCamera(H_field_target));
+ const Pose observed_target = nominal_target.Rebase(&observed_camera);
+ const Pose expected_target = nominal_target.Rebase(&expected_camera);
+ return Localizer::Corrector{
+ expected_robot_pose,
+ observed_camera,
+ expected_camera,
+ HeadingDistanceSkew(expected_target),
+ HeadingDistanceSkew(observed_target),
+ frc971::control_loops::drivetrain::HMatrixForCameraHeadingDistanceSkew(
+ nominal_target, observed_camera)};
+}
+
+Localizer::Corrector::Corrector(const State &state_at_capture,
+ const Transform &H_field_target,
+ const Transform &H_robot_camera,
+ const Transform &H_camera_target)
+ : Corrector(CalculateHeadingDistanceSkewH(
+ state_at_capture, H_field_target, H_robot_camera, H_camera_target)) {}
+
+Localizer::Output Localizer::Corrector::H(const State &, const Input &) {
+ return expected_ - observed_;
+}
+
+} // namespace y2024::localizer
diff --git a/y2024/localizer/localizer.h b/y2024/localizer/localizer.h
new file mode 100644
index 0000000..8235f13
--- /dev/null
+++ b/y2024/localizer/localizer.h
@@ -0,0 +1,158 @@
+#ifndef Y2024_LOCALIZER_LOCALIZER_H_
+#define Y2024_LOCALIZER_LOCALIZER_H_
+
+#include <array>
+#include <map>
+
+#include "aos/network/message_bridge_client_generated.h"
+#include "aos/network/message_bridge_server_generated.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/localization/utils.h"
+#include "frc971/imu_reader/imu_watcher.h"
+#include "frc971/vision/target_map_generated.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/localizer/status_generated.h"
+#include "y2024/localizer/visualization_static.h"
+
+namespace y2024::localizer {
+
+class Localizer {
+ public:
+ static constexpr size_t kNumCameras = 4;
+ using Pose = frc971::control_loops::Pose;
+ typedef Eigen::Matrix<double, 4, 4> Transform;
+ typedef frc971::control_loops::drivetrain::HybridEkf<double> HybridEkf;
+ typedef HybridEkf::State State;
+ typedef HybridEkf::Output Output;
+ typedef HybridEkf::Input Input;
+ typedef HybridEkf::StateIdx StateIdx;
+ Localizer(aos::EventLoop *event_loop);
+
+ private:
+ class Corrector : public HybridEkf::ExpectedObservationFunctor {
+ public:
+ // Indices used for each of the members of the output vector for this
+ // Corrector.
+ enum OutputIdx {
+ kHeading = 0,
+ kDistance = 1,
+ kSkew = 2,
+ };
+ Corrector(const State &state_at_capture, const Transform &H_field_target,
+ const Transform &H_robot_camera,
+ const Transform &H_camera_target);
+
+ using HMatrix = Eigen::Matrix<double, Localizer::HybridEkf::kNOutputs,
+ Localizer::HybridEkf::kNStates>;
+
+ Output H(const State &, const Input &) final;
+ HMatrix DHDX(const State &) final { return H_; }
+ const Eigen::Vector3d &observed() const { return observed_; }
+ const Eigen::Vector3d &expected() const { return expected_; }
+ const Pose &expected_robot_pose() const { return expected_robot_pose_; }
+ const Pose &expected_camera_pose() const { return expected_camera_; }
+ const Pose &observed_camera_pose() const { return observed_camera_; }
+
+ static Eigen::Vector3d HeadingDistanceSkew(const Pose &relative_pose);
+
+ static Corrector CalculateHeadingDistanceSkewH(
+ const State &state_at_capture, const Transform &H_field_target,
+ const Transform &H_robot_camera, const Transform &H_camera_target);
+
+ static void PopulateMeasurement(const Eigen::Vector3d &vector,
+ MeasurementStatic *builder) {
+ builder->set_heading(vector(kHeading));
+ builder->set_distance(vector(kDistance));
+ builder->set_skew(vector(kSkew));
+ }
+
+ private:
+ Corrector(const Pose &expected_robot_pose, const Pose &observed_camera,
+ const Pose &expected_camera, const Eigen::Vector3d &expected,
+ const Eigen::Vector3d &observed, const HMatrix &H)
+ : expected_robot_pose_(expected_robot_pose),
+ observed_camera_(observed_camera),
+ expected_camera_(expected_camera),
+ expected_(expected),
+ observed_(observed),
+ H_(H) {}
+ // For debugging.
+ const Pose expected_robot_pose_;
+ const Pose observed_camera_;
+ const Pose expected_camera_;
+ // Actually used.
+ const Eigen::Vector3d expected_;
+ const Eigen::Vector3d observed_;
+ const HMatrix H_;
+ };
+
+ struct CameraState {
+ aos::Sender<VisualizationStatic> debug_sender;
+ Transform extrinsics = Transform::Zero();
+ aos::util::ArrayErrorCounter<RejectionReason, RejectionCount>
+ rejection_counter;
+ size_t total_candidate_targets = 0;
+ size_t total_accepted_targets = 0;
+ };
+
+ static std::array<CameraState, kNumCameras> MakeCameras(
+ const Constants &constants, aos::EventLoop *event_loop);
+ void HandleTarget(int camera_index,
+ const aos::monotonic_clock::time_point capture_time,
+ const frc971::vision::TargetPoseFbs &target,
+ TargetEstimateDebugStatic *debug_builder);
+ void HandleImu(aos::monotonic_clock::time_point sample_time_pico,
+ aos::monotonic_clock::time_point sample_time_pi,
+ std::optional<Eigen::Vector2d> encoders, Eigen::Vector3d gyro,
+ Eigen::Vector3d accel);
+ void RejectImage(int camera_index, RejectionReason reason,
+ TargetEstimateDebugStatic *builder);
+
+ void SendOutput();
+ static flatbuffers::Offset<frc971::control_loops::drivetrain::LocalizerState>
+ PopulateState(const State &X_hat, flatbuffers::FlatBufferBuilder *fbb);
+ flatbuffers::Offset<ImuStatus> PopulateImu(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+ void SendStatus();
+ static flatbuffers::Offset<CumulativeStatistics> StatisticsForCamera(
+ const CameraState &camera, flatbuffers::FlatBufferBuilder *fbb);
+ static void StatisticsForCamera(const CameraState &camera,
+ CumulativeStatisticsStatic *builder);
+
+ bool UseAprilTag(uint64_t target_id);
+
+ aos::EventLoop *const event_loop_;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ std::array<CameraState, kNumCameras> cameras_;
+ const std::array<Transform, kNumCameras> camera_extrinsics_;
+ const std::map<uint64_t, Transform> target_poses_;
+
+ frc971::control_loops::drivetrain::DrivetrainUkf down_estimator_;
+ HybridEkf ekf_;
+ HybridEkf::ExpectedObservationAllocator<Corrector> observations_;
+
+ frc971::controls::ImuWatcher imu_watcher_;
+ frc971::control_loops::drivetrain::LocalizationUtils utils_;
+
+ aos::Sender<Status> status_sender_;
+ aos::Sender<frc971::controls::LocalizerOutput> output_sender_;
+ aos::monotonic_clock::time_point t_ = aos::monotonic_clock::min_time;
+ size_t clock_resets_ = 0;
+
+ size_t total_candidate_targets_ = 0;
+ size_t total_accepted_targets_ = 0;
+
+ // For the status message.
+ std::optional<Eigen::Vector2d> last_encoder_readings_;
+
+ aos::Fetcher<aos::message_bridge::ServerStatistics>
+ server_statistics_fetcher_;
+ aos::Fetcher<aos::message_bridge::ClientStatistics>
+ client_statistics_fetcher_;
+};
+} // namespace y2024::localizer
+#endif // Y2024_LOCALIZER_LOCALIZER_H_
diff --git a/y2024/localizer/localizer_logger.cc b/y2024/localizer/localizer_logger.cc
new file mode 100644
index 0000000..ec8d3ac
--- /dev/null
+++ b/y2024/localizer/localizer_logger.cc
@@ -0,0 +1,80 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/configuration.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/logging/snappy_encoder.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/log_namer.h"
+
+DEFINE_string(config, "aos_config.json", "Config file to use.");
+
+DEFINE_double(rotate_every, 30.0,
+ "If set, rotate the logger after this many seconds");
+
+DECLARE_int32(flush_size);
+
+int main(int argc, char *argv[]) {
+ gflags::SetUsageMessage(
+ "This program provides a simple logger binary that logs all SHMEM data "
+ "directly to a file specified at the command line. It does not manage "
+ "filenames, so it will just crash if you attempt to overwrite an "
+ "existing file, and the user must specify the logfile manually at the "
+ "command line.");
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ auto log_namer = std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
+ &event_loop,
+ std::make_unique<aos::logger::RenamableFileBackend>(
+ absl::StrCat(aos::logging::GetLogName("localizer_log"), "/"),
+ /*O_DIRECT*/ true));
+
+ log_namer->set_extension(aos::logger::SnappyDecoder::kExtension);
+ log_namer->set_encoder_factory([](size_t max_message_size) {
+ return std::make_unique<aos::logger::SnappyEncoder>(max_message_size,
+ FLAGS_flush_size);
+ });
+
+ aos::monotonic_clock::time_point last_rotation_time =
+ event_loop.monotonic_now();
+ aos::logger::Logger logger(
+ &event_loop, event_loop.configuration(),
+ // Only log channels smaller than ~10 MB / sec.
+ [](const aos::Channel *channel) {
+ return (channel->max_size() * channel->frequency()) < 10e6;
+ });
+
+ if (FLAGS_rotate_every != 0.0) {
+ logger.set_on_logged_period(
+ [&logger, &last_rotation_time](aos::monotonic_clock::time_point t) {
+ if (t > last_rotation_time +
+ std::chrono::duration<double>(FLAGS_rotate_every)) {
+ logger.Rotate();
+ last_rotation_time = t;
+ }
+ });
+ }
+
+ event_loop.OnRun([&log_namer, &logger]() {
+ errno = 0;
+ setpriority(PRIO_PROCESS, 0, -20);
+ PCHECK(errno == 0)
+ << ": Renicing to -20 failed, use --skip_renicing to skip renicing.";
+ logger.StartLogging(std::move(log_namer));
+ });
+
+ event_loop.Run();
+
+ LOG(INFO) << "Shutting down";
+
+ return 0;
+}
diff --git a/y2024/localizer/localizer_main.cc b/y2024/localizer/localizer_main.cc
new file mode 100644
index 0000000..25466d2
--- /dev/null
+++ b/y2024/localizer/localizer_main.cc
@@ -0,0 +1,23 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+#include "y2024/localizer/localizer.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
+ aos::ShmEventLoop event_loop(&config.message());
+ y2024::localizer::Localizer localizer(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024/localizer/localizer_plotter.ts b/y2024/localizer/localizer_plotter.ts
new file mode 100644
index 0000000..b5e764b
--- /dev/null
+++ b/y2024/localizer/localizer_plotter.ts
@@ -0,0 +1,203 @@
+// Provides a plot for debugging drivetrain-related issues.
+import {AosPlotter} from '../../aos/network/www/aos_plotter';
+import {ImuMessageHandler} from '../../frc971/wpilib/imu_plot_utils';
+import * as proxy from '../../aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from '../../aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+
+export function plotLocalizer(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+
+ const position = aosPlotter.addMessageSource("/drivetrain",
+ "frc971.control_loops.drivetrain.Position");
+ const status = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+ const output = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Output');
+ const localizer = aosPlotter.addMessageSource(
+ '/localizer', 'y2024.localizer.Status');
+ const imu = aosPlotter.addRawMessageSource(
+ '/localizer', 'frc971.IMUValuesBatch',
+ new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
+
+ // Drivetrain Status estimated relative position
+ const positionPlot = aosPlotter.addPlot(element);
+ positionPlot.plot.getAxisLabels().setTitle("Estimated Relative Position " +
+ "of the Drivetrain");
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel("Relative Position (m)");
+ const leftPosition =
+ positionPlot.addMessageLine(status, ["estimated_left_position"]);
+ leftPosition.setColor(RED);
+ const rightPosition =
+ positionPlot.addMessageLine(status, ["estimated_right_position"]);
+ rightPosition.setColor(GREEN);
+ positionPlot.addMessageLine(position, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(position, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+
+
+ // Drivetrain Velocities
+ const velocityPlot = aosPlotter.addPlot(element);
+ velocityPlot.plot.getAxisLabels().setTitle('Velocity Plots');
+ velocityPlot.plot.getAxisLabels().setXLabel(TIME);
+ velocityPlot.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
+
+ const leftVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_left_velocity']);
+ leftVelocity.setColor(RED);
+ const rightVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_right_velocity']);
+ rightVelocity.setColor(GREEN);
+
+ const leftSpeed = velocityPlot.addMessageLine(position, ["left_speed"]);
+ leftSpeed.setColor(BLUE);
+ const rightSpeed = velocityPlot.addMessageLine(position, ["right_speed"]);
+ rightSpeed.setColor(BROWN);
+
+ const ekfLeftVelocity = velocityPlot.addMessageLine(
+ localizer, ['state', 'left_velocity']);
+ ekfLeftVelocity.setColor(RED);
+ ekfLeftVelocity.setPointSize(0.0);
+ const ekfRightVelocity = velocityPlot.addMessageLine(
+ localizer, ['state', 'right_velocity']);
+ ekfRightVelocity.setColor(GREEN);
+ ekfRightVelocity.setPointSize(0.0);
+
+ // Lateral velocity
+ const lateralPlot = aosPlotter.addPlot(element);
+ lateralPlot.plot.getAxisLabels().setTitle('Lateral Velocity');
+ lateralPlot.plot.getAxisLabels().setXLabel(TIME);
+ lateralPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
+
+ lateralPlot.addMessageLine(localizer, ['state', 'lateral_velocity']).setColor(CYAN);
+
+ // Drivetrain Voltage
+ const voltagePlot = aosPlotter.addPlot(element);
+ voltagePlot.plot.getAxisLabels().setTitle('Voltage Plots');
+ voltagePlot.plot.getAxisLabels().setXLabel(TIME);
+ voltagePlot.plot.getAxisLabels().setYLabel('Voltage (V)')
+
+ voltagePlot.addMessageLine(localizer, ['state', 'left_voltage_error'])
+ .setColor(RED)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(localizer, ['state', 'right_voltage_error'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(output, ['left_voltage'])
+ .setColor(RED)
+ .setPointSize(0);
+ voltagePlot.addMessageLine(output, ['right_voltage'])
+ .setColor(GREEN)
+ .setPointSize(0);
+
+ // Heading
+ const yawPlot = aosPlotter.addPlot(element);
+ yawPlot.plot.getAxisLabels().setTitle('Robot Yaw');
+ yawPlot.plot.getAxisLabels().setXLabel(TIME);
+ yawPlot.plot.getAxisLabels().setYLabel('Yaw (rad)');
+
+ yawPlot.addMessageLine(status, ['localizer', 'theta']).setColor(GREEN);
+
+ yawPlot.addMessageLine(localizer, ['down_estimator', 'yaw']).setColor(BLUE);
+
+ yawPlot.addMessageLine(localizer, ['state', 'theta']).setColor(RED);
+
+ // Pitch/Roll
+ const orientationPlot = aosPlotter.addPlot(element);
+ orientationPlot.plot.getAxisLabels().setTitle('Orientation');
+ orientationPlot.plot.getAxisLabels().setXLabel(TIME);
+ orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
+
+ orientationPlot.addMessageLine(localizer, ['down_estimator', 'lateral_pitch'])
+ .setColor(RED)
+ .setLabel('roll');
+ orientationPlot
+ .addMessageLine(localizer, ['down_estimator', 'longitudinal_pitch'])
+ .setColor(GREEN)
+ .setLabel('pitch');
+
+ const stillPlot = aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 3]);
+ stillPlot.plot.getAxisLabels().setTitle('Still Plot');
+ stillPlot.plot.getAxisLabels().setXLabel(TIME);
+ stillPlot.plot.getAxisLabels().setYLabel('bool, g\'s');
+ stillPlot.plot.setDefaultYRange([-0.1, 1.1]);
+
+ stillPlot.addMessageLine(localizer, ['down_estimator', 'gravity_magnitude'])
+ .setColor(WHITE)
+ .setDrawLine(false);
+
+ // Absolute X Position
+ const xPositionPlot = aosPlotter.addPlot(element);
+ xPositionPlot.plot.getAxisLabels().setTitle('X Position');
+ xPositionPlot.plot.getAxisLabels().setXLabel(TIME);
+ xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
+
+ xPositionPlot.addMessageLine(status, ['x']).setColor(RED);
+ xPositionPlot.addMessageLine(localizer, ['down_estimator', 'position_x'])
+ .setColor(BLUE);
+ xPositionPlot.addMessageLine(localizer, ['state', 'x']).setColor(CYAN);
+
+ // Absolute Y Position
+ const yPositionPlot = aosPlotter.addPlot(element);
+ yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
+ yPositionPlot.plot.getAxisLabels().setXLabel(TIME);
+ yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
+
+ const localizerY = yPositionPlot.addMessageLine(status, ['y']);
+ localizerY.setColor(RED);
+ yPositionPlot.addMessageLine(localizer, ['down_estimator', 'position_y'])
+ .setColor(BLUE);
+ yPositionPlot.addMessageLine(localizer, ['state', 'y']).setColor(CYAN);
+
+ // Gyro
+ const gyroPlot = aosPlotter.addPlot(element);
+ gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
+ gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
+ gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
+ gyroX.setColor(RED);
+ const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
+ gyroY.setColor(GREEN);
+ const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
+ gyroZ.setColor(BLUE);
+
+
+ const timingPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ timingPlot.plot.getAxisLabels().setTitle('Fault Counting');
+ timingPlot.plot.getAxisLabels().setXLabel(TIME);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'imu_to_pico_checksum_mismatch'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'pico_to_pi_checksum_mismatch'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'other_zeroing_faults'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'missed_messages'])
+ .setColor(PINK)
+ .setDrawLine(false);
+}
diff --git a/y2024/localizer/localizer_replay.cc b/y2024/localizer/localizer_replay.cc
new file mode 100644
index 0000000..763ad36
--- /dev/null
+++ b/y2024/localizer/localizer_replay.cc
@@ -0,0 +1,68 @@
+#include "gflags/gflags.h"
+
+#include "aos/configuration.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/network/team_number.h"
+#include "aos/util/simulation_logger.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+#include "y2024/localizer/localizer.h"
+
+DEFINE_string(config, "y2024/aos_config.json",
+ "Name of the config file to replay using.");
+DEFINE_int32(team, 9971, "Team number to use for logfile replay.");
+DEFINE_string(output_folder, "/tmp/replayed",
+ "Name of the folder to write replayed logs to.");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::network::OverrideTeamNumber(FLAGS_team);
+
+ const aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ // sort logfiles
+ const std::vector<aos::logger::LogFile> logfiles =
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv));
+
+ // open logfiles
+ aos::logger::LogReader reader(logfiles, &config.message());
+
+ reader.RemapLoggedChannel("/localizer", "y2024.localizer.Status");
+ for (const auto orin : {"orin1", "orin2"}) {
+ for (const auto camera : {"camera0", "camera1"}) {
+ reader.RemapLoggedChannel(absl::StrCat("/", orin, "/", camera),
+ "y2024.localizer.Visualization");
+ }
+ }
+ reader.RemapLoggedChannel("/localizer", "frc971.controls.LocalizerOutput");
+
+ auto factory =
+ std::make_unique<aos::SimulatedEventLoopFactory>(reader.configuration());
+
+ reader.RegisterWithoutStarting(factory.get());
+
+ const aos::Node *node = nullptr;
+ if (aos::configuration::MultiNode(reader.configuration())) {
+ node = aos::configuration::GetNode(reader.configuration(), "imu");
+ }
+ std::vector<std::unique_ptr<aos::util::LoggerState>> loggers;
+
+ reader.OnStart(node, [&factory, node, &loggers]() {
+ aos::NodeEventLoopFactory *node_factory =
+ factory->GetNodeEventLoopFactory(node);
+ node_factory->AlwaysStart<y2024::localizer::Localizer>("localizer");
+ loggers.push_back(std::make_unique<aos::util::LoggerState>(
+ factory.get(), node, FLAGS_output_folder));
+ });
+
+ reader.event_loop_factory()->Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2024/localizer/localizer_test.cc b/y2024/localizer/localizer_test.cc
new file mode 100644
index 0000000..25c0bab
--- /dev/null
+++ b/y2024/localizer/localizer_test.cc
@@ -0,0 +1,554 @@
+#include "y2024/localizer/localizer.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/target_map_utils.h"
+#include "y2024/constants/simulated_constants_sender.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+#include "y2024/localizer/status_generated.h"
+
+DEFINE_string(output_folder, "",
+ "If set, logs all channels to the provided logfile.");
+DECLARE_double(max_distance_to_target);
+
+namespace y2024::localizer::testing {
+
+using frc971::control_loops::drivetrain::Output;
+
+class LocalizerTest : public ::testing::Test {
+ protected:
+ static constexpr uint64_t kTargetId = 1;
+ LocalizerTest()
+ : configuration_(aos::configuration::ReadConfig("y2024/aos_config.json")),
+ event_loop_factory_(&configuration_.message()),
+ roborio_node_([this]() {
+ // Get the constants sent before anything else happens.
+ // It has nothing to do with the roborio node.
+ SendSimulationConstants(&event_loop_factory_, 7971,
+ "y2024/constants/test_constants.json");
+ return aos::configuration::GetNode(&configuration_.message(),
+ "roborio");
+ }()),
+ imu_node_(
+ aos::configuration::GetNode(&configuration_.message(), "imu")),
+ camera_node_(
+ aos::configuration::GetNode(&configuration_.message(), "orin1")),
+ roborio_test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", roborio_node_)),
+ dt_config_(y2024::control_loops::drivetrain::GetDrivetrainConfig(
+ roborio_test_event_loop_.get())),
+ localizer_event_loop_(
+ event_loop_factory_.MakeEventLoop("localizer", imu_node_)),
+ localizer_(localizer_event_loop_.get()),
+ drivetrain_plant_event_loop_(event_loop_factory_.MakeEventLoop(
+ "drivetrain_plant", roborio_node_)),
+ drivetrain_plant_imu_event_loop_(
+ event_loop_factory_.MakeEventLoop("drivetrain_plant", imu_node_)),
+ drivetrain_plant_(drivetrain_plant_event_loop_.get(),
+ drivetrain_plant_imu_event_loop_.get(), dt_config_,
+ std::chrono::microseconds(1000)),
+ imu_test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", imu_node_)),
+ camera_test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", camera_node_)),
+ constants_fetcher_(imu_test_event_loop_.get()),
+ output_sender_(
+ roborio_test_event_loop_->MakeSender<Output>("/drivetrain")),
+ target_sender_(
+ camera_test_event_loop_->MakeSender<frc971::vision::TargetMap>(
+ "/camera0")),
+ control_sender_(roborio_test_event_loop_->MakeSender<
+ frc971::control_loops::drivetrain::LocalizerControl>(
+ "/drivetrain")),
+ output_fetcher_(
+ roborio_test_event_loop_
+ ->MakeFetcher<frc971::controls::LocalizerOutput>("/localizer")),
+ status_fetcher_(
+ imu_test_event_loop_->MakeFetcher<Status>("/localizer")) {
+ FLAGS_max_distance_to_target = 100.0;
+ {
+ aos::TimerHandler *timer = roborio_test_event_loop_->AddTimer([this]() {
+ {
+ auto builder = output_sender_.MakeBuilder();
+ auto output_builder = builder.MakeBuilder<Output>();
+ output_builder.add_left_voltage(output_voltages_(0));
+ output_builder.add_right_voltage(output_voltages_(1));
+ builder.CheckOk(builder.Send(output_builder.Finish()));
+ }
+ });
+ roborio_test_event_loop_->OnRun([timer, this]() {
+ timer->Schedule(roborio_test_event_loop_->monotonic_now(),
+ std::chrono::milliseconds(5));
+ });
+ }
+ {
+ // Sanity check that the test calibration files look like what we
+ // expect.
+ CHECK_EQ("orin1", constants_fetcher_.constants()
+ .cameras()
+ ->Get(0)
+ ->calibration()
+ ->node_name()
+ ->string_view());
+ CHECK_EQ(0, constants_fetcher_.constants()
+ .cameras()
+ ->Get(0)
+ ->calibration()
+ ->camera_number());
+ const Eigen::Matrix<double, 4, 4> H_robot_camera =
+ frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *constants_fetcher_.constants()
+ .cameras()
+ ->Get(0)
+ ->calibration()
+ ->fixed_extrinsics());
+
+ CHECK(constants_fetcher_.constants().common()->has_target_map());
+ CHECK(constants_fetcher_.constants()
+ .common()
+ ->target_map()
+ ->has_target_poses());
+ CHECK_LE(1u, constants_fetcher_.constants()
+ .common()
+ ->target_map()
+ ->target_poses()
+ ->size());
+ CHECK_EQ(kTargetId, constants_fetcher_.constants()
+ .common()
+ ->target_map()
+ ->target_poses()
+ ->Get(0)
+ ->id());
+ const Eigen::Matrix<double, 4, 4> H_field_target =
+ PoseToTransform(constants_fetcher_.constants()
+ .common()
+ ->target_map()
+ ->target_poses()
+ ->Get(0));
+ // For reference, the camera should pointed straight forwards on the
+ // robot, offset by 1 meter.
+ aos::TimerHandler *timer = camera_test_event_loop_->AddTimer(
+ [this, H_robot_camera, H_field_target]() {
+ if (!send_targets_) {
+ return;
+ }
+ const frc971::control_loops::Pose robot_pose(
+ {drivetrain_plant_.GetPosition().x(),
+ drivetrain_plant_.GetPosition().y(), 0.0},
+ drivetrain_plant_.state()(2, 0) + implied_yaw_error_);
+
+ const Eigen::Matrix<double, 4, 4> H_field_camera =
+ robot_pose.AsTransformationMatrix() * H_robot_camera;
+ const Eigen::Matrix<double, 4, 4> H_camera_target =
+ H_field_camera.inverse() * H_field_target;
+
+ Eigen::Quaterniond quat(H_camera_target.block<3, 3>(0, 0));
+ quat.normalize();
+ const Eigen::Vector3d translation(
+ H_camera_target.block<3, 1>(0, 3));
+
+ auto builder = target_sender_.MakeBuilder();
+ frc971::vision::Quaternion::Builder quat_builder(*builder.fbb());
+ quat_builder.add_w(quat.w());
+ quat_builder.add_x(quat.x());
+ quat_builder.add_y(quat.y());
+ quat_builder.add_z(quat.z());
+ auto quat_offset = quat_builder.Finish();
+ frc971::vision::Position::Builder position_builder(*builder.fbb());
+ position_builder.add_x(translation.x());
+ position_builder.add_y(translation.y());
+ position_builder.add_z(translation.z());
+ auto position_offset = position_builder.Finish();
+
+ frc971::vision::TargetPoseFbs::Builder target_builder(
+ *builder.fbb());
+ target_builder.add_id(send_target_id_);
+ target_builder.add_position(position_offset);
+ target_builder.add_orientation(quat_offset);
+ target_builder.add_pose_error(pose_error_);
+ target_builder.add_pose_error_ratio(pose_error_ratio_);
+ auto target_offset = target_builder.Finish();
+
+ auto targets_offset = builder.fbb()->CreateVector({target_offset});
+ frc971::vision::TargetMap::Builder map_builder(*builder.fbb());
+ map_builder.add_target_poses(targets_offset);
+ map_builder.add_monotonic_timestamp_ns(
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ camera_test_event_loop_->monotonic_now().time_since_epoch())
+ .count());
+
+ builder.CheckOk(builder.Send(map_builder.Finish()));
+ });
+ camera_test_event_loop_->OnRun([timer, this]() {
+ timer->Schedule(camera_test_event_loop_->monotonic_now(),
+ std::chrono::milliseconds(50));
+ });
+ }
+
+ localizer_control_send_timer_ =
+ roborio_test_event_loop_->AddTimer([this]() {
+ auto builder = control_sender_.MakeBuilder();
+ auto control_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerControl>();
+ control_builder.add_x(localizer_control_x_);
+ control_builder.add_y(localizer_control_y_);
+ control_builder.add_theta(localizer_control_theta_);
+ control_builder.add_theta_uncertainty(0.01);
+ control_builder.add_keep_current_theta(false);
+ builder.CheckOk(builder.Send(control_builder.Finish()));
+ });
+
+ // Get things zeroed.
+ event_loop_factory_.RunFor(std::chrono::seconds(10));
+ CHECK(status_fetcher_.Fetch());
+ CHECK(status_fetcher_->imu()->zeroed());
+
+ if (!FLAGS_output_folder.empty()) {
+ logger_event_loop_ =
+ event_loop_factory_.MakeEventLoop("logger", imu_node_);
+ logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+ logger_->StartLoggingOnRun(FLAGS_output_folder);
+ }
+ }
+
+ void SendLocalizerControl(double x, double y, double theta) {
+ localizer_control_x_ = x;
+ localizer_control_y_ = y;
+ localizer_control_theta_ = theta;
+ localizer_control_send_timer_->Schedule(
+ roborio_test_event_loop_->monotonic_now());
+ }
+
+ ::testing::AssertionResult IsNear(double expected, double actual,
+ double epsilon) {
+ if (std::abs(expected - actual) < epsilon) {
+ return ::testing::AssertionSuccess();
+ } else {
+ return ::testing::AssertionFailure()
+ << "Expected " << expected << " but got " << actual
+ << " with a max difference of " << epsilon
+ << " and an actual difference of " << std::abs(expected - actual);
+ }
+ }
+ ::testing::AssertionResult VerifyEstimatorAccurate(double eps) {
+ const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
+ ::testing::AssertionResult result(true);
+ status_fetcher_.Fetch();
+ if (!(result = IsNear(status_fetcher_->state()->x(), true_state(0), eps))) {
+ return result;
+ }
+ if (!(result = IsNear(status_fetcher_->state()->y(), true_state(1), eps))) {
+ return result;
+ }
+ if (!(result =
+ IsNear(status_fetcher_->state()->theta(), true_state(2), eps))) {
+ return result;
+ }
+ return result;
+ }
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ const aos::Node *const roborio_node_;
+ const aos::Node *const imu_node_;
+ const aos::Node *const camera_node_;
+ std::unique_ptr<aos::EventLoop> roborio_test_event_loop_;
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ std::unique_ptr<aos::EventLoop> localizer_event_loop_;
+ Localizer localizer_;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_imu_event_loop_;
+ frc971::control_loops::drivetrain::testing::DrivetrainSimulation
+ drivetrain_plant_;
+
+ std::unique_ptr<aos::EventLoop> imu_test_event_loop_;
+ std::unique_ptr<aos::EventLoop> camera_test_event_loop_;
+
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+
+ aos::Sender<Output> output_sender_;
+ aos::Sender<frc971::vision::TargetMap> target_sender_;
+ aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
+ control_sender_;
+ aos::Fetcher<frc971::controls::LocalizerOutput> output_fetcher_;
+ aos::Fetcher<Status> status_fetcher_;
+
+ Eigen::Vector2d output_voltages_ = Eigen::Vector2d::Zero();
+
+ aos::TimerHandler *localizer_control_send_timer_;
+
+ bool send_targets_ = false;
+
+ double localizer_control_x_ = 0.0;
+ double localizer_control_y_ = 0.0;
+ double localizer_control_theta_ = 0.0;
+
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::Logger> logger_;
+
+ uint64_t send_target_id_ = kTargetId;
+ double pose_error_ = 1e-7;
+ double pose_error_ratio_ = 0.1;
+ double implied_yaw_error_ = 0.0;
+
+ gflags::FlagSaver flag_saver_;
+};
+
+// Test a simple scenario with no errors where the robot should just drive
+// straight forwards.
+TEST_F(LocalizerTest, Nominal) {
+ output_voltages_ << 1.0, 1.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different
+ // times.
+ EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-2);
+ EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-6);
+ EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
+ 1e-6);
+ // Confirm that we did indeed drive forwards (and straight), as expected.
+ EXPECT_LT(0.1, output_fetcher_->x());
+ EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
+ EXPECT_NEAR(0.0, status_fetcher_->state()->left_voltage_error(), 1e-1);
+ EXPECT_NEAR(0.0, status_fetcher_->state()->right_voltage_error(), 1e-1);
+
+ // And check that we actually think that we are near where the simulator
+ // says we are.
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Confirm that when the robot drives backwards that we localize correctly.
+TEST_F(LocalizerTest, NominalReverse) {
+ output_voltages_ << -1.0, -1.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different
+ // times.
+ EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-2);
+ EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-6);
+ EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
+ 1e-6);
+ // Confirm that we did indeed drive backwards (and straight), as expected.
+ EXPECT_GT(-0.1, output_fetcher_->x());
+ EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
+ EXPECT_NEAR(0.0, status_fetcher_->state()->left_voltage_error(), 1e-1);
+ EXPECT_NEAR(0.0, status_fetcher_->state()->right_voltage_error(), 1e-1);
+
+ // And check that we actually think that we are near where the simulator
+ // says we are.
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Confirm that when the robot turns counter-clockwise that we localize
+// correctly.
+TEST_F(LocalizerTest, NominalSpinInPlace) {
+ output_voltages_ << -1.0, 1.0;
+ // Go 1 ms over 2 sec to make sure we actually see relatively recent messages
+ // on each channel.
+ event_loop_factory_.RunFor(std::chrono::milliseconds(2001));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different
+ // times.
+ EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 1e-6);
+ EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 1e-6);
+ EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
+ 1e-2);
+ // Confirm that we did indeed turn counter-clockwise.
+ EXPECT_NEAR(0.0, output_fetcher_->x(), 1e-10);
+ EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ EXPECT_LT(0.1, output_fetcher_->theta());
+ EXPECT_NEAR(0.0, status_fetcher_->state()->left_voltage_error(), 1e-1);
+ EXPECT_NEAR(0.0, status_fetcher_->state()->right_voltage_error(), 1e-1);
+
+ // And check that we actually think that we are near where the simulator
+ // says we are.
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Confirm that when the robot drives in a curve that we localize
+// successfully.
+TEST_F(LocalizerTest, NominalCurve) {
+ output_voltages_ << 2.0, 3.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different
+ // times.
+ EXPECT_NEAR(output_fetcher_->x(), status_fetcher_->state()->x(), 2e-2);
+ EXPECT_NEAR(output_fetcher_->y(), status_fetcher_->state()->y(), 2e-2);
+ EXPECT_NEAR(output_fetcher_->theta(), status_fetcher_->state()->theta(),
+ 2e-2);
+ // Confirm that we did indeed drive in a rough, counter-clockwise, curve.
+ EXPECT_LT(0.1, output_fetcher_->x());
+ EXPECT_LT(0.1, output_fetcher_->y());
+ EXPECT_LT(0.1, output_fetcher_->theta());
+
+ // And check that we actually think that we are near where the simulator
+ // says we are.
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Tests that, in the presence of a non-zero voltage error, that we correct
+// for it.
+TEST_F(LocalizerTest, VoltageErrorDisabled) {
+ output_voltages_ << 0.0, 0.0;
+ drivetrain_plant_.set_left_voltage_offset(2.0);
+ drivetrain_plant_.set_right_voltage_offset(2.0);
+
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // We should've just ended up driving straight forwards.
+ EXPECT_LT(0.1, output_fetcher_->x());
+ EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
+ EXPECT_NEAR(2.0, status_fetcher_->state()->left_voltage_error(), 1.0);
+ EXPECT_NEAR(2.0, status_fetcher_->state()->right_voltage_error(), 1.0);
+
+ // And check that we actually think that we are near where the simulator
+ // says we are.
+ EXPECT_TRUE(VerifyEstimatorAccurate(0.05));
+}
+
+// Tests that image corrections in the nominal case (no errors) causes no
+// issues.
+TEST_F(LocalizerTest, NominalImageCorrections) {
+ output_voltages_ << 3.0, 2.0;
+ send_targets_ = true;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+ ASSERT_TRUE(status_fetcher_->has_statistics());
+ ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
+ ASSERT_EQ(status_fetcher_->statistics()->Get(0)->total_candidates(),
+ status_fetcher_->statistics()->Get(0)->total_accepted());
+ ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
+// Tests that image corrections when there is an error at the start results
+// in us actually getting corrected over time.
+TEST_F(LocalizerTest, ImageCorrections) {
+ output_voltages_ << 0.0, 0.0;
+ // Put ourselves somewhat near the target so that we don't ignore its
+ // corrections too much.
+ drivetrain_plant_.mutable_state()->x() = 3.0;
+ drivetrain_plant_.mutable_state()->y() = -2.0;
+ SendLocalizerControl(5.0, 0.0, 0.0);
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ ASSERT_TRUE(output_fetcher_.Fetch());
+ EXPECT_NEAR(5.0, output_fetcher_->x(), 1e-5);
+ EXPECT_NEAR(0.0, output_fetcher_->y(), 1e-5);
+ EXPECT_NEAR(0.0, output_fetcher_->theta(), 1e-5);
+
+ send_targets_ = true;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(10));
+ CHECK(status_fetcher_.Fetch());
+ EXPECT_TRUE(VerifyEstimatorAccurate(0.1));
+ ASSERT_TRUE(status_fetcher_->has_statistics());
+ ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
+ ASSERT_EQ(status_fetcher_->statistics()->Get(0)->total_candidates(),
+ status_fetcher_->statistics()->Get(0)->total_accepted());
+ ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
+// Tests that we correctly reject an invalid target.
+TEST_F(LocalizerTest, InvalidTargetId) {
+ output_voltages_ << 0.0, 0.0;
+ send_targets_ = true;
+ send_target_id_ = 100;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->has_statistics());
+ ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
+ ASSERT_EQ(0, status_fetcher_->statistics()->Get(0)->total_accepted());
+ ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
+ ASSERT_EQ(status_fetcher_->statistics()
+ ->Get(0)
+ ->rejection_reasons()
+ ->Get(static_cast<size_t>(RejectionReason::NO_SUCH_TARGET))
+ ->count(),
+ status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
+// Tests that we correctly reject a detection with a high pose error.
+TEST_F(LocalizerTest, HighPoseError) {
+ output_voltages_ << 0.0, 0.0;
+ send_targets_ = true;
+ // Send the minimum pose error to be rejected
+ constexpr double kEps = 1e-9;
+ pose_error_ = 1e-6 + kEps;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->has_statistics());
+ ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
+ ASSERT_EQ(0, status_fetcher_->statistics()->Get(0)->total_accepted());
+ ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
+ ASSERT_EQ(status_fetcher_->statistics()
+ ->Get(0)
+ ->rejection_reasons()
+ ->Get(static_cast<size_t>(RejectionReason::HIGH_POSE_ERROR))
+ ->count(),
+ status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
+// Tests that we correctly reject a detection with a high implied yaw error.
+TEST_F(LocalizerTest, HighImpliedYawError) {
+ output_voltages_ << 0.0, 0.0;
+ send_targets_ = true;
+ implied_yaw_error_ = 31.0 * M_PI / 180.0;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->has_statistics());
+ ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
+ ASSERT_EQ(0, status_fetcher_->statistics()->Get(0)->total_accepted());
+ ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
+ ASSERT_EQ(
+ status_fetcher_->statistics()
+ ->Get(0)
+ ->rejection_reasons()
+ ->Get(static_cast<size_t>(RejectionReason::HIGH_IMPLIED_YAW_ERROR))
+ ->count(),
+ status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
+// Tests that we correctly reject a detection with a high pose error ratio.
+TEST_F(LocalizerTest, HighPoseErrorRatio) {
+ output_voltages_ << 0.0, 0.0;
+ send_targets_ = true;
+ // Send the minimum pose error to be rejected
+ constexpr double kEps = 1e-9;
+ pose_error_ratio_ = 0.4 + kEps;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->has_statistics());
+ ASSERT_EQ(4u /* number of cameras */, status_fetcher_->statistics()->size());
+ ASSERT_EQ(0, status_fetcher_->statistics()->Get(0)->total_accepted());
+ ASSERT_LT(10, status_fetcher_->statistics()->Get(0)->total_candidates());
+ ASSERT_EQ(
+ status_fetcher_->statistics()
+ ->Get(0)
+ ->rejection_reasons()
+ ->Get(static_cast<size_t>(RejectionReason::HIGH_POSE_ERROR_RATIO))
+ ->count(),
+ status_fetcher_->statistics()->Get(0)->total_candidates());
+}
+
+} // namespace y2024::localizer::testing
diff --git a/y2024/localizer/status.fbs b/y2024/localizer/status.fbs
new file mode 100644
index 0000000..ad2ac72
--- /dev/null
+++ b/y2024/localizer/status.fbs
@@ -0,0 +1,73 @@
+include "frc971/control_loops/drivetrain/drivetrain_status.fbs";
+include "frc971/imu_reader/imu_failures.fbs";
+
+namespace y2024.localizer;
+
+attribute "static_length";
+
+enum RejectionReason : uint8 {
+ // For some reason, the image timestamp indicates that the image was taken
+ // in the future.
+ IMAGE_FROM_FUTURE = 0,
+ // The image was too old for the buffer of old state estimates that we
+ // maintain.
+ IMAGE_TOO_OLD = 1,
+ // Message bridge is not yet connected, and so we can't get accurate
+ // time offsets betwee nnodes.
+ MESSAGE_BRIDGE_DISCONNECTED = 2,
+ // The target ID does not exist.
+ NO_SUCH_TARGET = 3,
+ // Pose estimation error was higher than any normal detection.
+ HIGH_POSE_ERROR = 4,
+ // Pose estimate implied a robot yaw far off from our estimate.
+ HIGH_IMPLIED_YAW_ERROR = 5,
+ // Pose estimate had a high distance to target.
+ // We don't trust estimates very far out.
+ HIGH_DISTANCE_TO_TARGET = 6,
+ // The robot was travelling too fast; we don't trust the target.
+ ROBOT_TOO_FAST = 7,
+ // Pose estimation error ratio was higher than any normal detection.
+ HIGH_POSE_ERROR_RATIO = 8,
+}
+
+table RejectionCount {
+ error:RejectionReason (id: 0);
+ count:uint (id: 1);
+}
+
+table CumulativeStatistics {
+ total_accepted:int (id: 0);
+ total_candidates:int (id: 1);
+ rejection_reasons:[RejectionCount] (id: 2, static_length: 9);
+}
+
+table ImuStatus {
+ // Whether the IMU is zeroed or not.
+ zeroed:bool (id: 0);
+ // Whether the IMU zeroing is faulted or not.
+ faulted_zero:bool (id: 1);
+ zeroing:frc971.control_loops.drivetrain.ImuZeroerState (id: 2);
+ // Offset between the IMU board clock and the orin clock, such that
+ // board_timestamp + board_offset_ns = orin_timestamp
+ board_offset_ns:int64 (id: 3);
+ // Error in the offset, if we assume that the orin/board clocks are
+ // identical and that there is a perfectly consistent latency between the
+ // two. Will be zero for the very first cycle, and then referenced off of
+ // the initial offset thereafter. If greater than zero, implies that the
+ // board is "behind", whether due to unusually large latency or due to
+ // clock drift.
+ board_offset_error_ns:int64 (id: 4);
+ left_encoder:double (id: 5);
+ right_encoder:double (id: 6);
+ imu_failures:frc971.controls.ImuFailures (id: 7);
+}
+
+table Status {
+ state: frc971.control_loops.drivetrain.LocalizerState (id: 0);
+ down_estimator:frc971.control_loops.drivetrain.DownEstimatorState (id: 1);
+ imu:ImuStatus (id: 2);
+ // Statistics are per-camera, by camera index.
+ statistics:[CumulativeStatistics] (id: 3);
+}
+
+root_type Status;
diff --git a/y2024/localizer/visualization.fbs b/y2024/localizer/visualization.fbs
new file mode 100644
index 0000000..d903e3a
--- /dev/null
+++ b/y2024/localizer/visualization.fbs
@@ -0,0 +1,47 @@
+include "y2024/localizer/status.fbs";
+
+namespace y2024.localizer;
+
+attribute "static_length";
+
+table Measurement {
+ heading:double (id: 0);
+ distance:double (id: 1);
+ skew:double (id: 2);
+}
+
+table TargetEstimateDebug {
+ camera:uint8 (id: 0);
+ camera_x:double (id: 1);
+ camera_y:double (id: 2);
+ camera_theta:double (id: 3);
+ implied_robot_x:double (id: 4);
+ implied_robot_y:double (id: 5);
+ implied_robot_theta:double (id: 6);
+ accepted:bool (id: 7);
+ rejection_reason:RejectionReason (id: 8);
+ // Image age (more human-readable than trying to interpret raw nanosecond
+ // values).
+ image_age_sec:double (id: 9);
+ // Time at which the image was captured.
+ image_monotonic_timestamp_ns:uint64 (id: 10);
+ // April tag ID used for this image detection.
+ april_tag:uint (id: 11);
+ // If the image was accepted, the total correction that occurred as a result.
+ // These numbers will be equal to the value after the correction - the value
+ // before.
+ correction_x: double (id: 12);
+ correction_y: double (id: 13);
+ correction_theta: double (id: 14);
+ // The expected observation given the current estimate of the robot pose.
+ expected_observation:Measurement (id: 15);
+ actual_observation:Measurement (id: 16);
+ modeled_noise:Measurement (id: 17);
+}
+
+table Visualization {
+ targets:[TargetEstimateDebug] (id: 0, static_length: 20);
+ statistics:CumulativeStatistics (id: 1);
+}
+
+root_type Visualization;
diff --git a/y2024/vision/image_logger.cc b/y2024/vision/image_logger.cc
index 45e25f6..55a4e12 100644
--- a/y2024/vision/image_logger.cc
+++ b/y2024/vision/image_logger.cc
@@ -25,7 +25,7 @@
std::unique_ptr<aos::logger::MultiNodeFilesLogNamer> MakeLogNamer(
aos::EventLoop *event_loop) {
std::optional<std::string> log_name =
- aos::logging::MaybeGetLogName("fbs_log");
+ aos::logging::MaybeGetLogName("image_log");
if (!log_name.has_value()) {
return nullptr;
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 921d7db..ece47d6 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -90,7 +90,7 @@
}
double turret_pot_translate(double voltage) {
- return voltage * Values::kTurretPotRadiansPerVolt();
+ return -1 * voltage * Values::kTurretPotRadiansPerVolt();
}
double altitude_pot_translate(double voltage) {
@@ -175,7 +175,7 @@
CopyPosition(extend_encoder_, builder->add_extend(),
Values::kExtendEncoderCountsPerRevolution(),
Values::kExtendEncoderMetersPerRadian(),
- extend_pot_translate, true,
+ extend_pot_translate, false,
robot_constants_->robot()
->extend_constants()
->potentiometer_offset());
@@ -204,6 +204,7 @@
->potentiometer_offset());
builder->set_transfer_beambreak(transfer_beam_break_->Get());
+ builder->set_extend_beambreak(extend_beam_break_->Get());
builder->set_catapult_beambreak(catapult_beam_break_->Get());
builder.CheckOk(builder.Send());
}
@@ -273,6 +274,10 @@
transfer_beam_break_ = ::std::move(sensor);
}
+ void set_extend_beambreak(::std::unique_ptr<frc::DigitalInput> sensor) {
+ extend_beam_break_ = ::std::move(sensor);
+ }
+
void set_catapult_beambreak(::std::unique_ptr<frc::DigitalInput> sensor) {
catapult_beam_break_ = ::std::move(sensor);
}
@@ -334,7 +339,7 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_, transfer_beam_break_,
- catapult_beam_break_;
+ extend_beam_break_, catapult_beam_break_;
frc971::wpilib::AbsoluteEncoder intake_pivot_encoder_;
frc971::wpilib::AbsoluteEncoderAndPotentiometer climber_encoder_,
@@ -416,20 +421,21 @@
SensorReader sensor_reader(&sensor_reader_event_loop, robot_constants);
sensor_reader.set_pwm_trigger(true);
sensor_reader.set_drivetrain_left_encoder(
- std::make_unique<frc::Encoder>(6, 7));
- sensor_reader.set_drivetrain_right_encoder(
std::make_unique<frc::Encoder>(8, 9));
+ sensor_reader.set_drivetrain_right_encoder(
+ std::make_unique<frc::Encoder>(6, 7));
sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(25));
sensor_reader.set_intake_pivot(make_encoder(3),
make_unique<frc::DigitalInput>(3));
sensor_reader.set_transfer_beambreak(make_unique<frc::DigitalInput>(23));
- sensor_reader.set_catapult_beambreak(make_unique<frc::DigitalInput>(24));
+ sensor_reader.set_extend_beambreak(make_unique<frc::DigitalInput>(24));
+ sensor_reader.set_catapult_beambreak(make_unique<frc::DigitalInput>(22));
- sensor_reader.set_climber(make_encoder(5),
- make_unique<frc::DigitalInput>(5),
- make_unique<frc::AnalogInput>(5));
- sensor_reader.set_extend(make_encoder(4), make_unique<frc::DigitalInput>(4),
- make_unique<frc::AnalogInput>(4));
+ sensor_reader.set_climber(make_encoder(4),
+ make_unique<frc::DigitalInput>(4),
+ make_unique<frc::AnalogInput>(4));
+ sensor_reader.set_extend(make_encoder(5), make_unique<frc::DigitalInput>(5),
+ make_unique<frc::AnalogInput>(5));
sensor_reader.set_catapult(make_encoder(0),
make_unique<frc::DigitalInput>(0),
make_unique<frc::AnalogInput>(0));
@@ -455,11 +461,11 @@
robot_constants->common()->current_limits();
std::shared_ptr<TalonFX> right_front = std::make_shared<TalonFX>(
- 2, false, "Drivetrain Bus", &canivore_signal_registry,
+ 2, true, "Drivetrain Bus", &canivore_signal_registry,
current_limits->drivetrain_supply_current_limit(),
current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> right_back = std::make_shared<TalonFX>(
- 1, false, "Drivetrain Bus", &canivore_signal_registry,
+ 1, true, "Drivetrain Bus", &canivore_signal_registry,
current_limits->drivetrain_supply_current_limit(),
current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> left_front = std::make_shared<TalonFX>(
@@ -479,9 +485,17 @@
current_limits->altitude_stator_current_limit(),
current_limits->altitude_supply_current_limit());
std::shared_ptr<TalonFX> turret = std::make_shared<TalonFX>(
- 3, false, "Drivetrain Bus", &canivore_signal_registry,
+ 3, true, "Drivetrain Bus", &canivore_signal_registry,
current_limits->turret_stator_current_limit(),
current_limits->turret_supply_current_limit());
+ std::shared_ptr<TalonFX> climber = std::make_shared<TalonFX>(
+ 7, true, "rio", &rio_signal_registry,
+ current_limits->climber_stator_current_limit(),
+ current_limits->climber_supply_current_limit());
+ std::shared_ptr<TalonFX> extend = std::make_shared<TalonFX>(
+ 12, false, "rio", &rio_signal_registry,
+ current_limits->extend_stator_current_limit(),
+ current_limits->extend_supply_current_limit());
std::shared_ptr<TalonFX> intake_roller = std::make_shared<TalonFX>(
8, false, "rio", &rio_signal_registry,
current_limits->intake_roller_stator_current_limit(),
@@ -491,21 +505,21 @@
current_limits->retention_roller_stator_current_limit(),
current_limits->retention_roller_supply_current_limit());
std::shared_ptr<TalonFX> transfer_roller = std::make_shared<TalonFX>(
- 9, true, "rio", &rio_signal_registry,
+ 11, true, "rio", &rio_signal_registry,
current_limits->transfer_roller_stator_current_limit(),
current_limits->transfer_roller_supply_current_limit());
- std::shared_ptr<TalonFX> climber = std::make_shared<TalonFX>(
- 7, false, "rio", &rio_signal_registry,
- current_limits->climber_stator_current_limit(),
- current_limits->climber_supply_current_limit());
- std::shared_ptr<TalonFX> extend = std::make_shared<TalonFX>(
- 11, false, "rio", &rio_signal_registry,
- current_limits->extend_stator_current_limit(),
- current_limits->extend_supply_current_limit());
std::shared_ptr<TalonFX> extend_roller = std::make_shared<TalonFX>(
- 12, true, "rio", &rio_signal_registry,
+ 13, true, "rio", &rio_signal_registry,
current_limits->extend_roller_stator_current_limit(),
current_limits->extend_roller_supply_current_limit());
+ std::shared_ptr<TalonFX> catapult_one = std::make_shared<TalonFX>(
+ 14, false, "Drivetrain Bus", &canivore_signal_registry,
+ current_limits->catapult_stator_current_limit(),
+ current_limits->catapult_supply_current_limit());
+ std::shared_ptr<TalonFX> catapult_two = std::make_shared<TalonFX>(
+ 15, false, "Drivetrain Bus", &canivore_signal_registry,
+ current_limits->catapult_stator_current_limit(),
+ current_limits->catapult_supply_current_limit());
ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
@@ -528,7 +542,8 @@
canivore_talonfxs.push_back(talonfx);
}
- for (auto talonfx : {intake_pivot, altitude, turret}) {
+ for (auto talonfx :
+ {intake_pivot, turret, altitude, catapult_one, catapult_two}) {
canivore_talonfxs.push_back(talonfx);
}
@@ -552,8 +567,9 @@
frc971::wpilib::CANSensorReader canivore_can_sensor_reader(
&can_sensor_reader_event_loop, std::move(canivore_signal_registry),
canivore_talonfxs,
- [drivetrain_talonfxs, &intake_pivot, &altitude, &turret,
- &drivetrain_can_position_sender, &superstructure_can_position_sender](
+ [drivetrain_talonfxs, &intake_pivot, &turret, &altitude, &catapult_one,
+ &catapult_two, &drivetrain_can_position_sender,
+ &superstructure_can_position_sender](
ctre::phoenix::StatusCode status) {
aos::Sender<frc971::control_loops::drivetrain::CANPositionStatic>::
StaticBuilder drivetrain_can_builder =
@@ -580,13 +596,19 @@
intake_pivot->SerializePosition(
superstructure_can_builder->add_intake_pivot(),
- control_loops::drivetrain::kHighOutputRatio);
- altitude->SerializePosition(
- superstructure_can_builder->add_altitude(),
- control_loops::drivetrain::kHighOutputRatio);
+ control_loops::superstructure::intake_pivot::kOutputRatio);
turret->SerializePosition(
superstructure_can_builder->add_turret(),
- control_loops::drivetrain::kHighOutputRatio);
+ control_loops::superstructure::turret::kOutputRatio);
+ altitude->SerializePosition(
+ superstructure_can_builder->add_altitude(),
+ control_loops::superstructure::altitude::kOutputRatio);
+ catapult_one->SerializePosition(
+ superstructure_can_builder->add_catapult_one(),
+ control_loops::superstructure::catapult::kOutputRatio);
+ catapult_two->SerializePosition(
+ superstructure_can_builder->add_catapult_two(),
+ control_loops::superstructure::catapult::kOutputRatio);
superstructure_can_builder->set_timestamp(
intake_pivot->GetTimestamp());
@@ -630,7 +652,8 @@
intake_roller->GetTimestamp());
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);
@@ -650,40 +673,43 @@
&talonfx_map) {
talonfx_map.find("intake_pivot")
->second->WriteVoltage(output.intake_pivot_voltage());
- talonfx_map.find("intake_roller")
- ->second->WriteVoltage(output.intake_roller_voltage());
- talonfx_map.find("transfer_roller")
- ->second->WriteVoltage(output.transfer_roller_voltage());
+ talonfx_map.find("altitude")
+ ->second->WriteVoltage(output.altitude_voltage());
+ talonfx_map.find("catapult_one")
+ ->second->WriteVoltage(output.catapult_voltage());
+ talonfx_map.find("catapult_two")
+ ->second->WriteVoltage(output.catapult_voltage());
+ talonfx_map.find("turret")->second->WriteVoltage(
+ output.turret_voltage());
talonfx_map.find("climber")->second->WriteVoltage(
output.climber_voltage());
talonfx_map.find("extend")->second->WriteVoltage(
output.extend_voltage());
+ talonfx_map.find("intake_roller")
+ ->second->WriteVoltage(output.intake_roller_voltage());
+ talonfx_map.find("transfer_roller")
+ ->second->WriteVoltage(output.transfer_roller_voltage());
talonfx_map.find("extend_roller")
->second->WriteVoltage(output.extend_roller_voltage());
- talonfx_map.find("altitude")
- ->second->WriteVoltage(output.altitude_voltage());
- talonfx_map.find("turret")->second->WriteVoltage(
- output.turret_voltage());
talonfx_map.find("retention_roller")
- ->second->WriteVoltage(output.retention_roller_voltage());
- if (output.has_retention_roller_stator_current_limit()) {
- talonfx_map.find("retention_roller")
- ->second->set_stator_current_limit(
- output.retention_roller_stator_current_limit());
- }
+ ->second->WriteCurrent(
+ output.retention_roller_stator_current_limit(),
+ output.retention_roller_voltage());
});
can_drivetrain_writer.set_talonfxs({right_front, right_back},
{left_front, left_back});
can_superstructure_writer.add_talonfx("intake_pivot", intake_pivot);
- can_superstructure_writer.add_talonfx("intake_roller", intake_roller);
- can_superstructure_writer.add_talonfx("transfer_roller", transfer_roller);
+ can_superstructure_writer.add_talonfx("altitude", altitude);
+ can_superstructure_writer.add_talonfx("catapult_one", catapult_one);
+ can_superstructure_writer.add_talonfx("catapult_two", catapult_two);
+ can_superstructure_writer.add_talonfx("turret", turret);
can_superstructure_writer.add_talonfx("climber", climber);
can_superstructure_writer.add_talonfx("extend", extend);
+ can_superstructure_writer.add_talonfx("intake_roller", intake_roller);
+ can_superstructure_writer.add_talonfx("transfer_roller", transfer_roller);
can_superstructure_writer.add_talonfx("extend_roller", extend_roller);
- can_superstructure_writer.add_talonfx("altitude", altitude);
- can_superstructure_writer.add_talonfx("turret", turret);
can_superstructure_writer.add_talonfx("retention_roller", retention_roller);
can_output_event_loop.MakeWatcher(
diff --git a/y2024/www/field.html b/y2024/www/field.html
index 8bdcf71..2013cee 100644
--- a/y2024/www/field.html
+++ b/y2024/www/field.html
@@ -44,6 +44,10 @@
<td id="superstructure_state"> NA </td>
</tr>
<tr>
+ <td>Catapult State</td>
+ <td id="catapult_state"> NA </td>
+ </tr>
+ <tr>
<td>Intake Roller State</td>
<td id="intake_roller_state"> NA </td>
</tr>
@@ -169,7 +173,7 @@
<td>Shot Distance</td>
<td id="shot_distance"> NA </td>
</tr>
- </table>
+ </table>
<h3>Zeroing Faults:</h3>
<p id="zeroing_faults"> NA </p>
diff --git a/y2024/www/field_handler.ts b/y2024/www/field_handler.ts
index fe13fcb..775eb1e 100644
--- a/y2024/www/field_handler.ts
+++ b/y2024/www/field_handler.ts
@@ -276,10 +276,25 @@
this.catapultState.innerHTML =
CatapultState[this.superstructureStatus.shooter().catapultState()];
- this.turret_position.innerHTML = this.superstructureStatus.shooter().aimer().turretPosition().toString();
- this.turret_velocity.innerHTML = this.superstructureStatus.shooter().aimer().turretVelocity().toString();
- this.target_distance.innerHTML = this.superstructureStatus.shooter().aimer().targetDistance().toString();
- this.shot_distance.innerHTML = this.superstructureStatus.shooter().aimer().shotDistance().toString();
+ if (this.superstructureStatus.shooter() &&
+ this.superstructureStatus.shooter().aimer()) {
+ this.turret_position.innerHTML = this.superstructureStatus.shooter()
+ .aimer()
+ .turretPosition()
+ .toString();
+ this.turret_velocity.innerHTML = this.superstructureStatus.shooter()
+ .aimer()
+ .turretVelocity()
+ .toString();
+ this.target_distance.innerHTML = this.superstructureStatus.shooter()
+ .aimer()
+ .targetDistance()
+ .toString();
+ this.shot_distance.innerHTML = this.superstructureStatus.shooter()
+ .aimer()
+ .shotDistance()
+ .toString();
+ }
if (!this.superstructureStatus.intakePivot() ||
!this.superstructureStatus.intakePivot().zeroed()) {
@@ -319,7 +334,7 @@
this.setEstopped(this.extend);
} else {
this.setTargetValue(
- this.climber,
+ this.extend,
this.superstructureStatus.extend().unprofiledGoalPosition(),
this.superstructureStatus.extend().estimatorState().position(),
1e-3);
diff --git a/y2024/www/plotter.html b/y2024/www/plotter.html
index 629ceaa..86f5aa8 100644
--- a/y2024/www/plotter.html
+++ b/y2024/www/plotter.html
@@ -1,6 +1,7 @@
<html>
<head>
<script src="plot_index_bundle.min.js" defer></script>
+ <link rel="stylesheet" href="styles.css">
</head>
<body>
</body>
diff --git a/y2024/www/styles.css b/y2024/www/styles.css
index 3259478..6193a57 100644
--- a/y2024/www/styles.css
+++ b/y2024/www/styles.css
@@ -78,4 +78,66 @@
display: table-cell;
padding: 5px;
text-align: right;
-}
\ No newline at end of file
+}
+
+.channel {
+ display: flex;
+ border-bottom: 1px solid;
+ font-size: 24px;
+}
+
+.aos_plot {
+ position: absolute;
+ width: 100%;
+ height: 100%;
+ box-sizing: border-box;
+}
+
+.aos_plot_text {
+ position: absolute;
+ width: 100%;
+ height: 100%;
+ pointer-events: none;
+}
+
+.aos_legend {
+ position: absolute;
+ z-index: 1;
+ pointer-events: none;
+}
+
+.aos_legend_line {
+ background: white;
+ padding: 2px;
+ border-radius: 2px;
+ margin-top: 3px;
+ margin-bottom: 3px;
+ font-size: 12;
+}
+
+.aos_legend_line>div {
+ display: inline-block;
+ vertical-align: middle;
+ margin-left: 5px;
+}
+.aos_legend_line>canvas {
+ vertical-align: middle;
+ pointer-events: all;
+}
+
+.aos_legend_line_hidden {
+ filter: contrast(0.75);
+}
+
+.aos_cpp_plot {
+ width: 100%;
+ display: flex;
+ flex-direction: column;
+ height: 100%;
+ align-items: flex-start;
+}
+
+.aos_cpp_plot>div {
+ flex: 1;
+ width: 100%;
+}
diff --git a/y2024/y2024.json b/y2024/y2024.json
index c901ac6..b55aebe 100644
--- a/y2024/y2024.json
+++ b/y2024/y2024.json
@@ -15,6 +15,5 @@
"y2024_roborio.json",
"y2024_imu.json",
"y2024_orin1.json",
- "y2024_orin2.json"
]
}
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index 72dc1f7..9ee0922 100644
--- a/y2024/y2024_imu.json
+++ b/y2024/y2024_imu.json
@@ -8,7 +8,6 @@
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
"orin1",
- "orin2"
],
"destination_nodes": [
{
@@ -20,15 +19,6 @@
"imu"
]
},
- {
- "name": "orin2",
- "priority": 5,
- "time_to_live": 50000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "imu"
- ]
- },
]
},
{
@@ -41,15 +31,6 @@
"max_size": 200
},
{
- "name": "/imu/aos/remote_timestamps/orin2/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",
@@ -70,7 +51,7 @@
"source_node": "imu",
"frequency": 50,
"num_senders": 20,
- "max_size": 2048
+ "max_size": 4096
},
{
"name": "/imu/aos",
@@ -190,6 +171,90 @@
"max_size": 200
},
{
+ "name": "/orin2/camera0",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "imu",
+ "channel_storage_duration": 1000000000,
+ "frequency": 65,
+ "max_size": 4752384,
+ "num_readers": 6,
+ "read_method": "PIN",
+ "num_senders": 18
+ },
+ {
+ "name": "/orin2/camera1",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "imu",
+ "channel_storage_duration": 1000000000,
+ "frequency": 65,
+ "max_size": 4752384,
+ "num_readers": 6,
+ "read_method": "PIN",
+ "num_senders": 18
+ },
+ {
+ "name": "/orin2/camera0",
+ "type": "foxglove.CompressedImage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "channel_storage_duration": 1000000000,
+ "frequency": 65,
+ "max_size": 622384
+ },
+ {
+ "name": "/orin2/camera1",
+ "type": "foxglove.CompressedImage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "channel_storage_duration": 1000000000,
+ "frequency": 65,
+ "max_size": 622384
+ },
+ {
+ "name": "/orin2/camera0",
+ "type": "foxglove.ImageAnnotations",
+ "source_node": "imu",
+ "frequency": 65,
+ "max_size": 50000
+ },
+ {
+ "name": "/orin2/camera1",
+ "type": "foxglove.ImageAnnotations",
+ "source_node": "imu",
+ "frequency": 65,
+ "max_size": 50000
+ },
+ {
+ "name": "/orin2/camera0",
+ "type": "y2024.localizer.Visualization",
+ "source_node": "imu",
+ "frequency": 65,
+ "max_size": 50000
+ },
+ {
+ "name": "/orin2/camera1",
+ "type": "y2024.localizer.Visualization",
+ "source_node": "imu",
+ "frequency": 65,
+ "max_size": 50000
+ },
+ {
+ "name": "/orin2/camera0",
+ "type": "frc971.vision.TargetMap",
+ "source_node": "imu",
+ "frequency": 65,
+ "num_senders": 2,
+ "max_size": 1024
+ },
+ {
+ "name": "/orin2/camera1",
+ "type": "frc971.vision.TargetMap",
+ "source_node": "imu",
+ "frequency": 65,
+ "num_senders": 2,
+ "max_size": 1024
+ },
+ {
"name": "/imu",
"type": "frc971.imu.DualImu",
"source_node": "imu",
@@ -246,6 +311,14 @@
"max_size": 200
},
{
+ "name": "/localizer",
+ "type": "y2024.localizer.Status",
+ "source_node": "imu",
+ "frequency": 1600,
+ "max_size": 1600,
+ "num_senders": 2
+ },
+ {
"name": "/imu/constants",
"type": "y2024.Constants",
"source_node": "imu",
@@ -285,12 +358,16 @@
]
},
{
+ "name": "localizer",
+ "executable_name": "localizer_main",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
"name": "localizer_logger",
- "executable_name": "logger_main",
- "args": [
- "--snappy_compress",
- "--rotate_every", "30.0"
- ],
+ "executable_name": "localizer_logger",
"user": "pi",
"nodes": [
"imu"
@@ -347,6 +424,105 @@
"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_camera0",
+ "executable_name": "argus_camera",
+ "args": [
+ "--enable_ftrace",
+ "--camera=0",
+ "--channel=/camera0",
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "argus_camera1",
+ "executable_name": "argus_camera",
+ "args": [
+ "--enable_ftrace",
+ "--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": [
@@ -367,17 +543,26 @@
"rename": {
"name": "/imu/aos"
}
+ },
+ {
+ "match": {
+ "name": "/camera*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/orin2/camera"
+ }
}
],
"nodes": [
{
"name": "imu",
- "hostname": "orin3",
+ "hostname": "orin2",
"hostnames": [
- "orin-971-3",
- "orin-7971-3",
- "orin-8971-3",
- "orin-9971-3"
+ "orin-971-2",
+ "orin-7971-2",
+ "orin-8971-2",
+ "orin-9971-2"
],
"port": 9971
},
@@ -387,8 +572,5 @@
{
"name": "orin1"
},
- {
- "name": "orin2"
- },
]
}
diff --git a/y2024/y2024_orin_template.json b/y2024/y2024_orin1.json
similarity index 72%
rename from y2024/y2024_orin_template.json
rename to y2024/y2024_orin1.json
index acbe568..a9f75d5 100644
--- a/y2024/y2024_orin_template.json
+++ b/y2024/y2024_orin1.json
@@ -1,61 +1,61 @@
{
"channels": [
{
- "name": "/orin{{ NUM }}/aos",
+ "name": "/orin1/aos",
"type": "aos.timing.Report",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 50,
"num_senders": 20,
"max_size": 8192
},
{
- "name": "/orin{{ NUM }}/aos",
+ "name": "/orin1/aos",
"type": "aos.logging.LogMessageFbs",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 200,
"num_senders": 20
},
{
- "name": "/orin{{ NUM }}/aos",
+ "name": "/orin1/aos",
"type": "aos.starter.Status",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 50,
"num_senders": 20,
"max_size": 2000
},
{
- "name": "/orin{{ NUM }}/aos",
+ "name": "/orin1/aos",
"type": "aos.starter.StarterRpc",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 10,
"num_senders": 2
},
{
- "name": "/orin{{ NUM }}/aos",
+ "name": "/orin1/aos",
"type": "aos.message_bridge.ServerStatistics",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"max_size": 2048,
"frequency": 10,
"num_senders": 2
},
{
- "name": "/orin{{ NUM }}/aos",
+ "name": "/orin1/aos",
"type": "aos.message_bridge.ClientStatistics",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 20,
"num_senders": 2
},
{
- "name": "/orin{{ NUM }}/aos",
+ "name": "/orin1/aos",
"type": "aos.logging.DynamicLogCommand",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 10,
"num_senders": 2
},
{
- "name": "/orin{{ NUM }}/aos",
+ "name": "/orin1/aos",
"type": "aos.message_bridge.Timestamp",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 15,
"num_senders": 2,
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -70,16 +70,16 @@
"time_to_live": 5000000,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
}
]
},
{
- "name": "/orin{{ NUM }}/aos/remote_timestamps/imu/orin{{ NUM }}/aos/aos-message_bridge-Timestamp",
+ "name": "/orin1/aos/remote_timestamps/imu/orin1/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 20,
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"max_size": 208
},
{
@@ -88,11 +88,11 @@
"source_node": "imu",
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "orin{{ NUM }}"
+ "orin1"
],
"destination_nodes": [
{
- "name": "orin{{ NUM }}",
+ "name": "orin1",
"priority": 1,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": [
@@ -103,16 +103,16 @@
]
},
{
- "name": "/imu/aos/remote_timestamps/orin{{ NUM }}/imu/aos/aos-message_bridge-Timestamp",
+ "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": "/orin{{ NUM }}/camera0",
+ "name": "/orin1/camera0",
"type": "frc971.vision.CameraImage",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"channel_storage_duration": 1000000000,
"frequency": 65,
"max_size": 4752384,
@@ -121,9 +121,9 @@
"num_senders": 18
},
{
- "name": "/orin{{ NUM }}/camera1",
+ "name": "/orin1/camera1",
"type": "frc971.vision.CameraImage",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"channel_storage_duration": 1000000000,
"frequency": 65,
"max_size": 4752384,
@@ -132,39 +132,53 @@
"num_senders": 18
},
{
- "name": "/orin{{ NUM }}/camera0",
+ "name": "/orin1/camera0",
"type": "foxglove.CompressedImage",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"channel_storage_duration": 1000000000,
"frequency": 65,
"max_size": 622384
},
{
- "name": "/orin{{ NUM }}/camera1",
+ "name": "/orin1/camera1",
"type": "foxglove.CompressedImage",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"channel_storage_duration": 1000000000,
"frequency": 65,
"max_size": 622384
},
{
- "name": "/orin{{ NUM }}/camera0",
+ "name": "/orin1/camera0",
"type": "foxglove.ImageAnnotations",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 65,
"max_size": 50000
},
{
- "name": "/orin{{ NUM }}/camera1",
+ "name": "/orin1/camera1",
"type": "foxglove.ImageAnnotations",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 65,
"max_size": 50000
},
{
- "name": "/orin{{ NUM }}/camera0",
+ "name": "/orin1/camera0",
+ "type": "y2024.localizer.Visualization",
+ "source_node": "imu",
+ "frequency": 65,
+ "max_size": 50000
+ },
+ {
+ "name": "/orin1/camera1",
+ "type": "y2024.localizer.Visualization",
+ "source_node": "imu",
+ "frequency": 65,
+ "max_size": 50000
+ },
+ {
+ "name": "/orin1/camera0",
"type": "frc971.vision.TargetMap",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 65,
"num_senders": 2,
"max_size": 1024,
@@ -178,16 +192,16 @@
"priority": 4,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": [
- "orin{{ NUM }}"
+ "orin1"
],
"time_to_live": 5000000
}
]
},
{
- "name": "/orin{{ NUM }}/camera1",
+ "name": "/orin1/camera1",
"type": "frc971.vision.TargetMap",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 65,
"num_senders": 2,
"max_size": 1024,
@@ -201,30 +215,30 @@
"priority": 4,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": [
- "orin{{ NUM }}"
+ "orin1"
],
"time_to_live": 5000000
}
]
},
{
- "name": "/orin{{ NUM }}/aos/remote_timestamps/imu/orin{{ NUM }}/camera0/frc971-vision-TargetMap",
+ "name": "/orin1/aos/remote_timestamps/imu/orin1/camera0/frc971-vision-TargetMap",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 80,
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"max_size": 208
},
{
- "name": "/orin{{ NUM }}/aos/remote_timestamps/imu/orin{{ NUM }}/camera1/frc971-vision-TargetMap",
+ "name": "/orin1/aos/remote_timestamps/imu/orin1/camera1/frc971-vision-TargetMap",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 80,
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"max_size": 208
},
{
- "name": "/orin{{ NUM }}/constants",
+ "name": "/orin1/constants",
"type": "y2024.Constants",
- "source_node": "orin{{ NUM }}",
+ "source_node": "orin1",
"frequency": 1,
"num_senders": 2,
"max_size": 65536
@@ -240,7 +254,7 @@
],
"user": "pi",
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -249,7 +263,7 @@
"user": "root",
"args": ["--user=pi"],
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -260,7 +274,7 @@
],
"user": "pi",
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -272,7 +286,7 @@
"--max_ice_port=5810"
],
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -286,14 +300,14 @@
],
"user": "pi",
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
"name": "foxglove_websocket",
"user": "pi",
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -304,7 +318,7 @@
"--channel", "/camera0"
],
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -315,7 +329,7 @@
"--channel", "/camera1"
],
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -323,7 +337,7 @@
"autorestart": false,
"user": "pi",
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -336,7 +350,7 @@
],
"user": "pi",
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -349,7 +363,7 @@
],
"user": "pi",
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -360,7 +374,7 @@
],
"user": "pi",
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
},
{
@@ -371,7 +385,7 @@
],
"user": "pi",
"nodes": [
- "orin{{ NUM }}"
+ "orin1"
]
}
],
@@ -379,40 +393,40 @@
{
"match": {
"name": "/aos*",
- "source_node": "orin{{ NUM }}"
+ "source_node": "orin1"
},
"rename": {
- "name": "/orin{{ NUM }}/aos"
+ "name": "/orin1/aos"
}
},
{
"match": {
"name": "/constants*",
- "source_node": "orin{{ NUM }}"
+ "source_node": "orin1"
},
"rename": {
- "name": "/orin{{ NUM }}/constants"
+ "name": "/orin1/constants"
}
},
{
"match": {
"name": "/camera*",
- "source_node": "orin{{ NUM }}"
+ "source_node": "orin1"
},
"rename": {
- "name": "/orin{{ NUM }}/camera"
+ "name": "/orin1/camera"
}
}
],
"nodes": [
{
- "name": "orin{{ NUM }}",
- "hostname": "orin{{ NUM }}",
+ "name": "orin1",
+ "hostname": "orin1",
"hostnames": [
- "orin-971-{{ NUM }}",
- "orin-7971-{{ NUM }}",
- "orin-8971-{{ NUM }}",
- "orin-9971-{{ NUM }}"
+ "orin-971-1",
+ "orin-7971-1",
+ "orin-8971-1",
+ "orin-9971-1"
],
"port": 9971
},
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
index 5ddb254..8584484 100644
--- a/y2024/y2024_roborio.json
+++ b/y2024/y2024_roborio.json
@@ -127,6 +127,7 @@
"type": "y2024.control_loops.superstructure.Status",
"source_node": "roborio",
"frequency": 400,
+ "max_size": 2048,
"num_senders": 2
},
{
@@ -151,7 +152,7 @@
"source_node": "roborio",
"frequency": 220,
"num_senders": 2,
- "max_size": 608
+ "max_size": 1024
},
{
"name": "/superstructure/rio",
@@ -159,7 +160,7 @@
"source_node": "roborio",
"frequency": 220,
"num_senders": 2,
- "max_size": 608
+ "max_size": 1024
},
{
"name": "/can",
@@ -221,7 +222,18 @@
"source_node": "roborio",
"frequency": 400,
"max_size": 128,
- "num_senders": 2
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
},
{
"name": "/drivetrain",