Add Extend Constants
Signed-off-by: Mirabel Wang <mirabel.17.wang@gmail.com>
Change-Id: Iac9692569b52fb9fd121cb3ab45e513202ddbba6
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