Swap y2023 wrist over to the constants.json
There are a couple of awkward constants that show up in json_codegen.cc
as still being managed from the C++ end; there are also a couple of
codegen'd constants from the wrist python code that we use in
constants.h. However, this does move *all* of the per-robot
configuration into the constants JSON file and should allow us to remove
the superstructure's dependency on the constants.h (if the arm were also
converted over).
Change-Id: Id9fc1e80830af823d96e5f3c1e469495eba454a4
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2023/BUILD b/y2023/BUILD
index 2fb9e78..9bbed4f 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -285,6 +285,7 @@
"//aos/util:wrapping_counter",
"//frc971:can_configuration_fbs",
"//frc971/autonomous:auto_mode_fbs",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops:control_loop",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
@@ -306,6 +307,7 @@
"//third_party:phoenix",
"//third_party:phoenix6",
"//third_party:wpilib",
+ "//y2023/constants:constants_fbs",
"//y2023/control_loops/superstructure:led_indicator_lib",
"//y2023/control_loops/superstructure:superstructure_output_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2023/constants.cc b/y2023/constants.cc
index 0154453..0b89ef0 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -23,7 +23,6 @@
Values r;
auto *const arm_proximal = &r.arm_proximal;
auto *const arm_distal = &r.arm_distal;
- auto *const wrist = &r.wrist;
auto *const roll_joint = &r.roll_joint;
r.wrist_flipped = true;
@@ -46,23 +45,6 @@
roll_joint->zeroing.moving_buffer_size = 20;
roll_joint->zeroing.allowable_encoder_error = 0.9;
- wrist->subsystem_params.zeroing_voltage = 3.0;
- wrist->subsystem_params.operating_voltage = 12.0;
- wrist->subsystem_params.zeroing_profile_params = {{}, 0.5, 3.0};
- wrist->subsystem_params.default_profile_params = {{}, 0.5, 5.0};
- wrist->subsystem_params.range = Values::kCompWristRange();
- wrist->subsystem_params.make_integral_loop =
- control_loops::superstructure::wrist::MakeIntegralWristLoop;
- wrist->subsystem_params.zeroing_constants.average_filter_size =
- Values::kZeroingSampleSize;
- wrist->subsystem_params.zeroing_constants.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kCompWristEncoderRatio();
- wrist->subsystem_params.zeroing_constants.zeroing_threshold = 0.0005;
- wrist->subsystem_params.zeroing_constants.moving_buffer_size = 20;
- wrist->subsystem_params.zeroing_constants.allowable_encoder_error = 0.9;
- wrist->subsystem_params.zeroing_constants.middle_position =
- Values::kCompWristRange().middle();
-
switch (team) {
// A set of constants for tests.
case 1:
@@ -77,9 +59,6 @@
roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
- wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.0;
-
break;
case kCompTeamNumber:
@@ -109,9 +88,6 @@
0.097581301615046 + 3.3424421683095 - 3.97605190912604 +
0.709274294168941 - 0.0817908884966825 + 0.0420732537514303;
- wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.616272675539221;
-
break;
case kPracticeTeamNumber:
@@ -147,9 +123,6 @@
1.09682107821155 - 0.193945964842277 + 0.811834321668829 -
0.913134567575683;
- wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 2.94344206522199;
-
break;
case kCodingRobotTeamNumber:
@@ -162,9 +135,6 @@
roll_joint->zeroing.measured_absolute_position = 0.0;
roll_joint->potentiometer_offset = 0.0;
- wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.0;
-
break;
default:
diff --git a/y2023/constants.h b/y2023/constants.h
index d90bbc9..92428db 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -225,8 +225,6 @@
ArmJointConstants arm_distal;
ArmJointConstants roll_joint;
- AbsEncoderConstants wrist;
-
bool wrist_flipped;
};
diff --git a/y2023/constants/971.json b/y2023/constants/971.json
index fc21b74..bc26de8 100644
--- a/y2023/constants/971.json
+++ b/y2023/constants/971.json
@@ -25,6 +25,10 @@
"lateral_position": -0.23
}
]
+ },
+ "wrist_zero": {
+ {% include 'y2023/constants/wrist_common_zeroing.json' %},
+ "measured_absolute_position": 0.616272675539221
}
},
{% include 'y2023/constants/common.json' %}
diff --git a/y2023/constants/9971.json b/y2023/constants/9971.json
index d687c9e..4b10956 100644
--- a/y2023/constants/9971.json
+++ b/y2023/constants/9971.json
@@ -25,6 +25,10 @@
"lateral_position": -0.23
}
]
+ },
+ "wrist_zero": {
+ {% include 'y2023/constants/wrist_common_zeroing.json' %},
+ "measured_absolute_position": 2.94344206522199
}
},
{% include 'y2023/constants/common.json' %}
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
index 512e400..bbcec56 100644
--- a/y2023/constants/BUILD
+++ b/y2023/constants/BUILD
@@ -1,4 +1,5 @@
load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("@aspect_bazel_lib//lib:run_binary.bzl", "run_binary")
load("//tools/build_rules:template.bzl", "jinja2_template")
cc_library(
@@ -19,7 +20,12 @@
jinja2_template(
name = "test_constants.json",
src = "test_constants.jinja2.json",
- includes = glob(["test_data/*.json"]),
+ includes = glob([
+ "test_data/*.json",
+ ]) + [
+ ":wrist_common_zeroing.json",
+ "//y2023/control_loops/superstructure/wrist:wrist_json",
+ ],
parameters = {},
visibility = ["//visibility:public"],
)
@@ -33,6 +39,8 @@
"9971.json",
"common.json",
":scoring_map",
+ ":wrist_common_zeroing.json",
+ "//y2023/control_loops/superstructure/wrist:wrist_json",
"//y2023/vision/calib_files",
"//y2023/vision/maps",
],
@@ -54,8 +62,11 @@
srcs = ["constants.fbs"],
visibility = ["//visibility:public"],
deps = [
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops:state_feedback_loop_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
+ "//frc971/zeroing:constants_fbs",
"//y2023/localizer:scoring_map_fbs",
],
)
@@ -80,3 +91,19 @@
"//frc971/constants:constants_sender_lib",
],
)
+
+cc_binary(
+ name = "json_codegen",
+ srcs = ["json_codegen.cc"],
+ deps = [
+ "//aos/util:file",
+ "//y2023:constants",
+ ],
+)
+
+run_binary(
+ name = "wrist_json_codegen",
+ outs = ["wrist_common_zeroing.json"],
+ args = ["$(location :wrist_common_zeroing.json)"],
+ tool = ":json_codegen",
+)
diff --git a/y2023/constants/common.json b/y2023/constants/common.json
index d63c518..a120e52 100644
--- a/y2023/constants/common.json
+++ b/y2023/constants/common.json
@@ -3,4 +3,24 @@
"ignore_targets": {
"red": [4],
"blue": [5]
+ },
+ "wrist": {
+ "zeroing_voltage": 3.0,
+ "operating_voltage": 12.0,
+ "zeroing_profile_params": {
+ "max_velocity": 0.5,
+ "max_acceleration": 3.0
+ },
+ "default_profile_params":{
+ "max_velocity": 0.5,
+ "max_acceleration": 5.0
+ },
+ {# The range is currently duplicated from the constants.h; try to fix this. #}
+ "range": {
+ "lower_hard": -0.10,
+ "upper_hard": 4.90,
+ "lower": 0.0,
+ "upper": 4.0
+ },
+ "loop": {% include 'y2023/control_loops/superstructure/wrist/integral_wrist_plant.json' %}
}
diff --git a/y2023/constants/constants.fbs b/y2023/constants/constants.fbs
index 61c3365..8910051 100644
--- a/y2023/constants/constants.fbs
+++ b/y2023/constants/constants.fbs
@@ -1,5 +1,7 @@
include "frc971/vision/calibration.fbs";
include "frc971/vision/target_map.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/zeroing/constants.fbs";
include "y2023/localizer/scoring_map.fbs";
namespace y2023;
@@ -27,6 +29,7 @@
// Table of time-of-flight reading positions. Until we bother using one
// of our interpolation classes, should just contain two values.
tof:TimeOfFlight (id: 0);
+ wrist_zero:frc971.zeroing.AbsoluteEncoderZeroingConstants (id: 1);
}
// Set of april tag targets, by april tag ID, to ignore when on a
@@ -42,6 +45,7 @@
scoring_map:localizer.ScoringMap (id: 2);
robot:RobotConstants (id: 3);
ignore_targets:IgnoreTargets (id: 4);
+ wrist:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 5);
}
root_type Constants;
diff --git a/y2023/constants/json_codegen.cc b/y2023/constants/json_codegen.cc
new file mode 100644
index 0000000..fc08df1
--- /dev/null
+++ b/y2023/constants/json_codegen.cc
@@ -0,0 +1,25 @@
+#include "aos/util/file.h"
+#include "y2023/constants.h"
+
+using namespace y2023::constants;
+
+// This file generates some JSON constants information that is currently
+// dependent on values that are located in C++ headers and would be
+// obnoxious/inappropriate to pull out.
+int main(int argc, char *argv[]) {
+ CHECK_EQ(argc, 2) << "Must supply file name to output to.";
+ std::string output_file = argv[1];
+
+ std::stringstream output;
+
+ output << "\"average_filter_size\": " << Values::kZeroingSampleSize << ",\n";
+ output << "\"one_revolution_distance\": "
+ << M_PI * 2.0 * Values::kCompWristEncoderRatio() << ",\n";
+ output << "\"zeroing_threshold\": 0.0005,\n";
+ output << "\"moving_buffer_size\": 20,\n";
+ output << "\"allowable_encoder_error\": 0.9,\n";
+ output << "\"middle_position\": " << Values::kCompWristRange().middle()
+ << "\n";
+ aos::util::WriteStringToFileOrDie(output_file, output.str());
+ return 0;
+}
diff --git a/y2023/constants/test_data/test_team.json b/y2023/constants/test_data/test_team.json
index 0a226f0..bb9c1e4 100644
--- a/y2023/constants/test_data/test_team.json
+++ b/y2023/constants/test_data/test_team.json
@@ -27,8 +27,32 @@
"lateral_position": -0.2
}
]
+ },
+ "wrist_zero": {
+ {% include 'y2023/constants/wrist_common_zeroing.json' %},
+ "measured_absolute_position": 0.0
}
},
+ "wrist": {
+ "zeroing_voltage": 3.0,
+ "operating_voltage": 12.0,
+ "zeroing_profile_params": {
+ "max_velocity": 0.5,
+ "max_acceleration": 3.0
+ },
+ "default_profile_params":{
+ "max_velocity": 0.5,
+ "max_acceleration": 5.0
+ },
+ {# The range is currently duplicated from the constants.h; try to fix this. #}
+ "range": {
+ "lower_hard": -0.10,
+ "upper_hard": 4.90,
+ "lower": 0.0,
+ "upper": 4.0
+ },
+ "loop": {% include 'y2023/control_loops/superstructure/wrist/integral_wrist_plant.json' %}
+ },
"ignore_targets": {
"red": [4],
"blue": [5]
diff --git a/y2023/control_loops/python/wrist.py b/y2023/control_loops/python/wrist.py
index fe2fcf6..3ad404c 100644
--- a/y2023/control_loops/python/wrist.py
+++ b/y2023/control_loops/python/wrist.py
@@ -41,13 +41,13 @@
return
# Write the generated constants out to a file.
- if len(argv) != 5:
+ if len(argv) != 7:
glog.fatal(
'Expected .h file name and .cc file name for the wrist and integral wrist.'
)
else:
namespaces = ['y2023', 'control_loops', 'superstructure', 'wrist']
- angular_system.WriteAngularSystem(kWrist, argv[1:3], argv[3:5],
+ angular_system.WriteAngularSystem(kWrist, argv[1:4], argv[4:7],
namespaces)
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index e11092a..0ee1061 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -34,7 +34,8 @@
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
arm_(values_, arm_trajectories.message()),
end_effector_(),
- wrist_(values->wrist.subsystem_params) {
+ wrist_(constants_fetcher_.constants().wrist(),
+ constants_fetcher_.constants().robot()->wrist_zero()) {
event_loop->SetRuntimeRealtimePriority(30);
}
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index d4fa653..5973eee 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -175,15 +175,23 @@
chrono::nanoseconds dt)
: event_loop_(event_loop),
dt_(dt),
+ constants_fetcher_(event_loop),
arm_(values->arm_proximal.zeroing, values->arm_distal.zeroing,
values->roll_joint.zeroing, dt_),
wrist_(new CappedTestPlant(wrist::MakeWristPlant()),
- PositionSensorSimulator(
- values->wrist.subsystem_params.zeroing_constants
- .one_revolution_distance),
- values->wrist, constants::Values::kCompWristRange(),
- values->wrist.subsystem_params.zeroing_constants
- .measured_absolute_position,
+ PositionSensorSimulator(constants_fetcher_.constants()
+ .robot()
+ ->wrist_zero()
+ ->one_revolution_distance()),
+ {.subsystem_params =
+ {constants_fetcher_.constants().wrist(),
+ constants_fetcher_.constants().robot()->wrist_zero()}},
+ frc971::constants::Range::FromFlatbuffer(
+ constants_fetcher_.constants().wrist()->range()),
+ constants_fetcher_.constants()
+ .robot()
+ ->wrist_zero()
+ ->measured_absolute_position(),
dt_),
superstructure_position_sender_(
event_loop_->MakeSender<Position>("/superstructure")),
@@ -257,6 +265,7 @@
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
ArmSimulation arm_;
AbsoluteEncoderSimulator wrist_;
diff --git a/y2023/control_loops/superstructure/wrist/BUILD b/y2023/control_loops/superstructure/wrist/BUILD
index 694d95f..212c203 100644
--- a/y2023/control_loops/superstructure/wrist/BUILD
+++ b/y2023/control_loops/superstructure/wrist/BUILD
@@ -5,8 +5,10 @@
outs = [
"wrist_plant.h",
"wrist_plant.cc",
+ "wrist_plant.json",
"integral_wrist_plant.h",
"integral_wrist_plant.cc",
+ "integral_wrist_plant.json",
],
cmd = "$(location //y2023/control_loops/python:wrist) $(OUTS)",
target_compatible_with = ["@platforms//os:linux"],
@@ -32,3 +34,9 @@
"//frc971/control_loops:state_feedback_loop",
],
)
+
+filegroup(
+ name = "wrist_json",
+ srcs = ["integral_wrist_plant.json"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 8438281..d03bda3 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -39,6 +39,7 @@
#include "aos/util/wrapping_counter.h"
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/can_configuration_generated.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/input/robot_state_generated.h"
@@ -56,6 +57,7 @@
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2023/constants.h"
+#include "y2023/constants/constants_generated.h"
#include "y2023/control_loops/superstructure/led_indicator.h"
#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023/control_loops/superstructure/superstructure_position_static.h"
@@ -403,6 +405,7 @@
CANSensorReader *can_sensor_reader)
: ::frc971::wpilib::SensorReader(event_loop),
values_(std::move(values)),
+ constants_fetcher_(event_loop),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
"/autonomous")),
@@ -460,8 +463,10 @@
false, values_->roll_joint.potentiometer_offset);
CopyPosition(wrist_encoder_, builder->add_wrist(),
Values::kWristEncoderCountsPerRevolution(),
- values_->wrist.subsystem_params.zeroing_constants
- .one_revolution_distance /
+ constants_fetcher_.constants()
+ .robot()
+ ->wrist_zero()
+ ->one_revolution_distance() /
(M_PI * 2.0),
values_->wrist_flipped);
@@ -632,6 +637,7 @@
private:
std::shared_ptr<const Values> values_;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
aos::Sender<superstructure::PositionStatic> superstructure_position_sender_;
@@ -919,6 +925,8 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig("aos_config.json");
+ frc971::constants::WaitForConstants<y2023::Constants>(&config.message());
+
// Thread 1.
::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(