Add Extend Constants
Signed-off-by: Mirabel Wang <mirabel.17.wang@gmail.com>
Change-Id: Iac9692569b52fb9fd121cb3ab45e513202ddbba6
diff --git a/y2024/BUILD b/y2024/BUILD
index 875bf84..d8d0db7 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -212,6 +212,7 @@
"//frc971/zeroing:pot_and_absolute_encoder",
"//y2024/control_loops/drivetrain:polydrivetrain_plants",
"//y2024/control_loops/superstructure/climber:climber_plants",
+ "//y2024/control_loops/superstructure/extend:extend_plants",
"//y2024/control_loops/superstructure/intake_pivot:intake_pivot_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
diff --git a/y2024/constants.h b/y2024/constants.h
index 44c5c23..a5bef55 100644
--- a/y2024/constants.h
+++ b/y2024/constants.h
@@ -12,6 +12,7 @@
#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2024/control_loops/superstructure/climber/climber_plant.h"
+#include "y2024/control_loops/superstructure/extend/extend_plant.h"
#include "y2024/control_loops/superstructure/intake_pivot/intake_pivot_plant.h"
namespace y2024::constants {
@@ -89,6 +90,24 @@
kClimberEncoderRatio() * kClimberEncoderCountsPerRevolution();
}
+ static constexpr double kExtendEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kExtendEncoderRatio() { return 1.0; }
+
+ static constexpr double kExtendPotMetersPerRevolution() {
+ return 36 * 0.005 * kExtendEncoderRatio();
+ }
+ static constexpr double kExtendEncoderMetersPerRevolution() {
+ return kExtendPotMetersPerRevolution();
+ }
+ static constexpr double kExtendPotMetersPerVolt() {
+ return kExtendPotMetersPerRevolution() * (5.0 /*turns*/ / 5.0 /*volts*/);
+ }
+ static constexpr double kMaxExtendEncoderPulsesPerSecond() {
+ return control_loops::superstructure::extend::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::extend::kOutputRatio /
+ kExtendEncoderRatio() * kExtendEncoderCountsPerRevolution();
+ }
struct PotAndAbsEncoderConstants {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
diff --git a/y2024/constants/7971.json b/y2024/constants/7971.json
index 6ce6771..ec59033 100644
--- a/y2024/constants/7971.json
+++ b/y2024/constants/7971.json
@@ -3,6 +3,7 @@
{% from 'y2024/constants/common.jinja2' import catapult_zero %}
{% from 'y2024/constants/common.jinja2' import altitude_zero %}
{% from 'y2024/constants/common.jinja2' import turret_zero %}
+{% from 'y2024/constants/common.jinja2' import extend_zero %}
{
"cameras": [
@@ -52,6 +53,15 @@
) %}
"zeroing_constants": {{ turret_zero | tojson(indent=2)}},
"potentiometer_offset": 0.0
+ },
+ "extend_constants": {
+ {% set _ = extend_zero.update(
+ {
+ "measured_absolute_position" : 0.0
+ }
+ ) %}
+ "zeroing_constants": {{ extend_zero | tojson(indent=2)}},
+ "potentiometer_offset": 0.0
}
},
{% include 'y2024/constants/common.json' %}
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 6ce6771..ec59033 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -3,6 +3,7 @@
{% from 'y2024/constants/common.jinja2' import catapult_zero %}
{% from 'y2024/constants/common.jinja2' import altitude_zero %}
{% from 'y2024/constants/common.jinja2' import turret_zero %}
+{% from 'y2024/constants/common.jinja2' import extend_zero %}
{
"cameras": [
@@ -52,6 +53,15 @@
) %}
"zeroing_constants": {{ turret_zero | tojson(indent=2)}},
"potentiometer_offset": 0.0
+ },
+ "extend_constants": {
+ {% set _ = extend_zero.update(
+ {
+ "measured_absolute_position" : 0.0
+ }
+ ) %}
+ "zeroing_constants": {{ extend_zero | tojson(indent=2)}},
+ "potentiometer_offset": 0.0
}
},
{% include 'y2024/constants/common.json' %}
diff --git a/y2024/constants/9971.json b/y2024/constants/9971.json
index 3c69701..ec59033 100644
--- a/y2024/constants/9971.json
+++ b/y2024/constants/9971.json
@@ -3,6 +3,7 @@
{% from 'y2024/constants/common.jinja2' import catapult_zero %}
{% from 'y2024/constants/common.jinja2' import altitude_zero %}
{% from 'y2024/constants/common.jinja2' import turret_zero %}
+{% from 'y2024/constants/common.jinja2' import extend_zero %}
{
"cameras": [
@@ -52,8 +53,16 @@
) %}
"zeroing_constants": {{ turret_zero | tojson(indent=2)}},
"potentiometer_offset": 0.0
+ },
+ "extend_constants": {
+ {% set _ = extend_zero.update(
+ {
+ "measured_absolute_position" : 0.0
+ }
+ ) %}
+ "zeroing_constants": {{ extend_zero | tojson(indent=2)}},
+ "potentiometer_offset": 0.0
}
},
{% include 'y2024/constants/common.json' %}
}
-
diff --git a/y2024/constants/BUILD b/y2024/constants/BUILD
index 737b976..bcdd34a 100644
--- a/y2024/constants/BUILD
+++ b/y2024/constants/BUILD
@@ -26,6 +26,7 @@
"//y2024/control_loops/superstructure/climber:climber_json",
"//y2024/control_loops/superstructure/catapult:catapult_json",
"//y2024/control_loops/superstructure/altitude:altitude_json",
+ "//y2024/control_loops/superstructure/extend:extend_json",
"//y2024/control_loops/superstructure/turret:turret_json",
"//y2024/control_loops/drivetrain:drivetrain_config.json",
"//y2024/constants/calib_files",
@@ -51,6 +52,7 @@
"//y2024/control_loops/superstructure/altitude:altitude_json",
"//y2024/control_loops/superstructure/catapult:catapult_json",
"//y2024/control_loops/superstructure/climber:climber_json",
+ "//y2024/control_loops/superstructure/extend:extend_json",
"//y2024/control_loops/superstructure/intake_pivot:intake_pivot_json",
"//y2024/control_loops/superstructure/turret:turret_json",
"//y2024/vision/maps",
diff --git a/y2024/constants/common.jinja2 b/y2024/constants/common.jinja2
index cb663fa..e132993 100644
--- a/y2024/constants/common.jinja2
+++ b/y2024/constants/common.jinja2
@@ -16,6 +16,18 @@
}
%}
+{% set extend_encoder_ratio = (1.0 / 1.0) %}
+{% set extend_radius = 36.0 * 0.005 / 2.0 / pi %}
+{%
+set extend_zero = {
+ "average_filter_size": zeroing_sample_size,
+ "one_revolution_distance": pi * 2.0 * extend_encoder_ratio * extend_radius,
+ "zeroing_threshold": 0.0005,
+ "moving_buffer_size": 20,
+ "allowable_encoder_error": 0.9
+}
+%}
+
{% set climber_encoder_ratio = (16.0 / 60.0) %}
{% set climber_radius = 16.0 * 0.25 / 2.0 / pi %}
{%
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index ddd1a27..6e175c7 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -140,5 +140,24 @@
"upper": 1.57
},
"loop": {% include 'y2024/control_loops/superstructure/turret/integral_turret_plant.json' %}
+ },
+ "extend": {
+ "zeroing_voltage": 3.0,
+ "operating_voltage": 12.0,
+ "zeroing_profile_params": {
+ "max_velocity": 0.5,
+ "max_acceleration": 3.0
+ },
+ "default_profile_params":{
+ "max_velocity": 6.0,
+ "max_acceleration": 20.0
+ },
+ "range": {
+ "lower_hard": -0.85,
+ "upper_hard": 1.85,
+ "lower": -0.400,
+ "upper": 1.57
+ },
+ "loop": {% include 'y2024/control_loops/superstructure/extend/integral_extend_plant.json' %}
}
}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index c47f277..d96df45 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -78,12 +78,19 @@
transfer_out:double (id: 1);
}
+// Extend positions
+table ExtendSetPoints {
+ extended:double (id: 0);
+ retracted:double (id: 1);
+}
+
table RobotConstants {
intake_constants:frc971.zeroing.AbsoluteEncoderZeroingConstants (id: 0);
climber_constants:PotAndAbsEncoderConstants (id: 1);
catapult_constants:PotAndAbsEncoderConstants (id: 2);
altitude_constants:PotAndAbsEncoderConstants (id: 3);
turret_constants:PotAndAbsEncoderConstants (id: 4);
+ extend_constants:PotAndAbsEncoderConstants (id: 5);
}
// Common table for constants unrelated to the robot
@@ -104,6 +111,8 @@
catapult:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 12);
altitude:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 13);
turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 14);
+ extend:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 15);
+ extend_setpoints:ExtendSetPoints (id: 16);
}
table Constants {
diff --git a/y2024/constants/constants_validator_test.cc b/y2024/constants/constants_validator_test.cc
index cd478a8..79dedf6 100644
--- a/y2024/constants/constants_validator_test.cc
+++ b/y2024/constants/constants_validator_test.cc
@@ -8,10 +8,11 @@
class ConstantsValidatorTest : public ::testing::Test {};
TEST_F(ConstantsValidatorTest, CheckConstants) {
- CHECK_NOTNULL(aos::JsonFileToFlatbuffer<y2024::ConstantsList>(
- "y2024/constants/constants.json")
- .message()
- .constants());
+ ASSERT_TRUE(aos::JsonFileToFlatbuffer<y2024::ConstantsList>(
+ "y2024/constants/constants.json")
+ .message()
+ .constants() != nullptr)
+ << ": Failed to parse y2024/constants/constants.json";
}
} // namespace y2024::constants::testing
diff --git a/y2024/control_loops/python/BUILD b/y2024/control_loops/python/BUILD
index 93d4fe2..9cbfcf8 100644
--- a/y2024/control_loops/python/BUILD
+++ b/y2024/control_loops/python/BUILD
@@ -122,6 +122,22 @@
],
)
+py_binary(
+ name = "extend",
+ srcs = [
+ "extend.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//frc971/control_loops/python:controls",
+ "//frc971/control_loops/python:linear_system",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
diff --git a/y2024/control_loops/python/extend.py b/y2024/control_loops/python/extend.py
new file mode 100644
index 0000000..e84b45e
--- /dev/null
+++ b/y2024/control_loops/python/extend.py
@@ -0,0 +1,58 @@
+#!/usr/bin/python3
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import linear_system
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+gflags.DEFINE_bool('hybrid', False, 'If true, make it hybrid.')
+
+kExtend = linear_system.LinearSystemParams(
+ name='Extend',
+ motor=control_loop.KrakenFOC(),
+ G=(14. / 60.) * (32. / 48.),
+ radius=36 * 0.005 / numpy.pi / 2.0,
+ mass=5.0,
+ q_pos=0.40,
+ q_vel=3.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05,
+)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ linear_system.PlotKick(kExtend, R)
+ linear_system.PlotMotion(kExtend, R)
+ return
+
+ # Write the generated constants out to a file.
+ if len(argv) != 7:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the extend pivot and integral extend pivot.'
+ )
+ else:
+ namespaces = ['y2024', 'control_loops', 'superstructure', 'extend']
+ linear_system.WriteLinearSystem(kExtend, argv[1:4], argv[4:7],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2024/control_loops/superstructure/extend/BUILD b/y2024/control_loops/superstructure/extend/BUILD
new file mode 100644
index 0000000..7a28bde
--- /dev/null
+++ b/y2024/control_loops/superstructure/extend/BUILD
@@ -0,0 +1,42 @@
+package(default_visibility = ["//y2024:__subpackages__"])
+
+genrule(
+ name = "genrule_extend",
+ outs = [
+ "extend_plant.h",
+ "extend_plant.cc",
+ "extend_plant.json",
+ "integral_extend_plant.h",
+ "integral_extend_plant.cc",
+ "integral_extend_plant.json",
+ ],
+ cmd = "$(location //y2024/control_loops/python:extend) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2024/control_loops/python:extend",
+ ],
+)
+
+cc_library(
+ name = "extend_plants",
+ srcs = [
+ "extend_plant.cc",
+ "integral_extend_plant.cc",
+ ],
+ hdrs = [
+ "extend_plant.h",
+ "integral_extend_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
+
+filegroup(
+ name = "extend_json",
+ srcs = ["integral_extend_plant.json"],
+ visibility = ["//visibility:public"],
+)