Add y2024 intake constants
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I41914cb76b7d3f79b7d9a0f0f2bab1632fa583be
diff --git a/y2024/BUILD b/y2024/BUILD
index be8abe0..e00b19a 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -206,6 +206,7 @@
"//frc971/zeroing:absolute_encoder",
"//frc971/zeroing:pot_and_absolute_encoder",
"//y2024/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2024/control_loops/superstructure/intake_pivot:intake_pivot_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
@@ -231,6 +232,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",
@@ -253,6 +255,7 @@
"//third_party:phoenix",
"//third_party:phoenix6",
"//third_party:wpilib",
+ "//y2024/constants:constants_fbs",
"//y2024/control_loops/superstructure:superstructure_output_fbs",
"//y2024/control_loops/superstructure:superstructure_position_fbs",
],
diff --git a/y2024/constants.cc b/y2024/constants.cc
index a347353..a5194e2 100644
--- a/y2024/constants.cc
+++ b/y2024/constants.cc
@@ -36,8 +36,6 @@
default:
LOG(FATAL) << "unknown team: " << team;
-
- // TODO(milind): add pot range checks once we add ranges
}
return r;
diff --git a/y2024/constants.h b/y2024/constants.h
index b9e5a7d..45fd8b0 100644
--- a/y2024/constants.h
+++ b/y2024/constants.h
@@ -11,6 +11,7 @@
#include "frc971/zeroing/absolute_encoder.h"
#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024/control_loops/superstructure/intake_pivot/intake_pivot_plant.h"
namespace y2024 {
namespace constants {
@@ -54,6 +55,36 @@
return (rotations * (2.0 * M_PI)) *
control_loops::drivetrain::kHighOutputRatio;
}
+ // TODO: (niko) add the gear ratios for the intake once we have them
+ static constexpr double kIntakePivotEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+
+ static constexpr double kIntakePivotEncoderRatio() {
+ return (16.0 / 64.0) * (18.0 / 62.0);
+ }
+
+ static constexpr double kIntakePivotPotRatio() { return 16.0 / 64.0; }
+
+ static constexpr double kIntakePivotPotRadiansPerVolt() {
+ return kIntakePivotPotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+ }
+
+ static constexpr double kMaxIntakePivotEncoderPulsesPerSecond() {
+ return control_loops::superstructure::intake_pivot::kFreeSpeed /
+ (2.0 * M_PI) *
+ control_loops::superstructure::intake_pivot::kOutputRatio /
+ kIntakePivotEncoderRatio() *
+ kIntakePivotEncoderCountsPerRevolution();
+ }
+
+ struct PotAndAbsEncoderConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ subsystem_params;
+ double potentiometer_offset;
+ };
};
// Creates and returns a Values instance for the constants.
diff --git a/y2024/constants/7971.json b/y2024/constants/7971.json
index 0062fa8..62d4b32 100644
--- a/y2024/constants/7971.json
+++ b/y2024/constants/7971.json
@@ -1,5 +1,12 @@
{
"robot": {
+ "intake_constants": {
+ "intake_pivot_zero": {
+ "measured_absolute_position": 0.0,
+ {% include 'y2024/constants/intake_pivot_common_zeroing.json' %}
+ },
+ "potentiometer_offset": 0.0
+ }
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 0062fa8..62d4b32 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -1,5 +1,12 @@
{
"robot": {
+ "intake_constants": {
+ "intake_pivot_zero": {
+ "measured_absolute_position": 0.0,
+ {% include 'y2024/constants/intake_pivot_common_zeroing.json' %}
+ },
+ "potentiometer_offset": 0.0
+ }
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/constants/9971.json b/y2024/constants/9971.json
index 0062fa8..62d4b32 100644
--- a/y2024/constants/9971.json
+++ b/y2024/constants/9971.json
@@ -1,5 +1,12 @@
{
"robot": {
+ "intake_constants": {
+ "intake_pivot_zero": {
+ "measured_absolute_position": 0.0,
+ {% include 'y2024/constants/intake_pivot_common_zeroing.json' %}
+ },
+ "potentiometer_offset": 0.0
+ }
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/constants/BUILD b/y2024/constants/BUILD
index af01182..adebcf9 100644
--- a/y2024/constants/BUILD
+++ b/y2024/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,14 @@
jinja2_template(
name = "test_constants.json",
src = "test_constants.jinja2.json",
- includes = glob(["test_data/*.json"]),
+ includes = glob([
+ "test_data/*.json",
+ ]) + [
+ ":intake_pivot_common_zeroing.json",
+ "//y2024/control_loops/superstructure/intake_pivot:intake_pivot_json",
+ "common.json",
+ "//y2024/vision/maps",
+ ],
parameters = {},
visibility = ["//visibility:public"],
)
@@ -32,6 +40,8 @@
"971.json",
"9971.json",
"common.json",
+ ":intake_pivot_common_zeroing.json",
+ "//y2024/control_loops/superstructure/intake_pivot:intake_pivot_json",
"//y2024/vision/maps",
],
parameters = {},
@@ -43,7 +53,9 @@
srcs = ["constants.fbs"],
visibility = ["//visibility:public"],
deps = [
+ "//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/vision:target_map_fbs",
+ "//frc971/zeroing:constants_fbs",
],
)
@@ -80,3 +92,19 @@
"@com_github_google_glog//:glog",
],
)
+
+cc_binary(
+ name = "intake_pivot_json_codegen",
+ srcs = ["intake_pivot_json_codegen.cc"],
+ deps = [
+ "//aos/util:file",
+ "//y2024:constants",
+ ],
+)
+
+run_binary(
+ name = "intake_pivot_codegen",
+ outs = ["intake_pivot_common_zeroing.json"],
+ args = ["$(location :intake_pivot_common_zeroing.json)"],
+ tool = ":intake_pivot_json_codegen",
+)
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 279ec04..701ddd1 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -1,4 +1,6 @@
-"shooter_interpolation_table": [
+"common": {
+ "target_map": {% include 'y2024/vision/maps/target_map.json' %},
+ "shooter_interpolation_table": [
{
"distance_from_goal": 0.0,
"shot_params": {
@@ -6,5 +8,34 @@
"shot_angle": 0.0
}
}
-],
-"target_map": {% include 'y2024/vision/maps/target_map.json' %}
+ ],
+ "intake_roller_voltages": {
+ "spitting": -12.0,
+ "intaking": 12.0
+ },
+ "intake_pivot_set_points": {
+ "extended": 0.0,
+ "retracted": 0.0
+ },
+ "intake_pivot": {
+ "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": 30.0
+ },
+ "range": {
+ "lower_hard": -0.85,
+ "upper_hard": 1.85,
+ "lower": -0.400,
+ "upper": 1.57
+ },
+ "loop": {% include 'y2024/control_loops/superstructure/intake_pivot/integral_intake_pivot_plant.json' %}
+ }
+}
+
+
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index 490bc99..52a54bb 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -1,4 +1,6 @@
include "frc971/vision/target_map.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/zeroing/constants.fbs";
namespace y2024;
@@ -12,14 +14,44 @@
shot_params: ShotParams (id: 1);
}
-table RobotConstants {
+// Amount of voltage to give to the intake rollers when:
+// spitting, which represents voltage when IntakeRollerGoal is SPITTING
+// and intaking, which represents voltage when IntakeRollerGoal is INTAKING
+table IntakeRollerVoltages {
+ spitting:double (id: 0);
+ intaking:double (id: 1);
+}
+// Set points for the pivot in radians when:
+// extended, which represents radians for when IntakePivotGoal is EXTENDED
+// and retracted, which represents radians for when IntakePivotGoal is RETRACTED or NONE
+table IntakePivotSetPoints {
+ extended:double (id: 0);
+ retracted:double (id: 1);
+}
+
+// Intake Constants
+table IntakeConstants {
+ intake_pivot_zero:frc971.zeroing.PotAndAbsoluteEncoderZeroingConstants (id: 0);
+ potentiometer_offset:double (id: 1);
+}
+
+table RobotConstants {
+ intake_constants:IntakeConstants (id: 0);
+}
+
+// Common table for constants unrelated to the robot
+table Common {
+ target_map:frc971.vision.TargetMap (id: 0);
+ shooter_interpolation_table: [InterpolationTablePoint] (id: 1);
+ intake_roller_voltages:IntakeRollerVoltages (id : 2);
+ intake_pivot_set_points:IntakePivotSetPoints (id: 3);
+ intake_pivot:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 4);
}
table Constants {
- target_map:frc971.vision.TargetMap (id: 0);
- robot:RobotConstants (id: 1);
- shooter_interpolation_table: [InterpolationTablePoint] (id: 2);
+ robot:RobotConstants (id: 0);
+ common:Common (id: 1);
}
-root_type Constants;
+root_type Constants;
\ No newline at end of file
diff --git a/y2024/constants/intake_pivot_json_codegen.cc b/y2024/constants/intake_pivot_json_codegen.cc
new file mode 100644
index 0000000..9a0b6dd
--- /dev/null
+++ b/y2024/constants/intake_pivot_json_codegen.cc
@@ -0,0 +1,24 @@
+#include "aos/util/file.h"
+#include "y2024/constants.h"
+
+using namespace y2024::constants;
+
+// This file generates some JSON constants information for the intake that are
+// currently dependent on values that are located in C++ headers and would be
+// obnoxious/inappropriate to pull out. In the future the file may be used to
+// generate constants information for other subsystems as well
+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::kIntakePivotEncoderRatio() << ",\n";
+ output << "\"zeroing_threshold\": 0.0005,\n";
+ output << "\"moving_buffer_size\": 20,\n";
+ output << "\"allowable_encoder_error\": 0.9\n";
+ aos::util::WriteStringToFileOrDie(output_file, output.str());
+ return 0;
+}
\ No newline at end of file
diff --git a/y2024/constants/test_constants.jinja2.json b/y2024/constants/test_constants.jinja2.json
index c6b77dd..aa5c553 100644
--- a/y2024/constants/test_constants.jinja2.json
+++ b/y2024/constants/test_constants.jinja2.json
@@ -2,7 +2,7 @@
"constants": [
{
"team": 7971,
- "data": {}
+ "data": {% include 'y2024/constants/test_data/test_team.json' %}
}
]
}
diff --git a/y2024/constants/test_data/test_team.json b/y2024/constants/test_data/test_team.json
new file mode 100644
index 0000000..bb30395
--- /dev/null
+++ b/y2024/constants/test_data/test_team.json
@@ -0,0 +1,12 @@
+{
+ "robot": {
+ "intake_constants": {
+ "intake_pivot_zero": {
+ "measured_absolute_position": 0.0,
+ {% include 'y2024/constants/intake_pivot_common_zeroing.json' %}
+ },
+ "potentiometer_offset": 0.0
+ }
+ },
+ {% include 'y2024/constants/common.json' %}
+}
\ No newline at end of file
diff --git a/y2024/control_loops/python/BUILD b/y2024/control_loops/python/BUILD
index 4451fb9..f92b484 100644
--- a/y2024/control_loops/python/BUILD
+++ b/y2024/control_loops/python/BUILD
@@ -45,6 +45,22 @@
],
)
+py_binary(
+ name = "intake_pivot",
+ srcs = [
+ "intake_pivot.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//frc971/control_loops/python:angular_system",
+ "//frc971/control_loops/python:controls",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
diff --git a/y2024/control_loops/python/intake_pivot.py b/y2024/control_loops/python/intake_pivot.py
new file mode 100644
index 0000000..9e7543b
--- /dev/null
+++ b/y2024/control_loops/python/intake_pivot.py
@@ -0,0 +1,56 @@
+#!/usr/bin/python3
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import angular_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
+
+kIntakePivot = angular_system.AngularSystemParams(
+ name='IntakePivot',
+ motor=control_loop.KrakenFOC(),
+ # TODO(Niko): Change gear ratios when we have all of them
+ G=0.02,
+ J=0.34,
+ q_pos=0.40,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05,
+ radius=13 * 0.0254)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system.PlotKick(kIntakePivot, R)
+ angular_system.PlotMotion(kIntakePivot, R)
+ return
+ if len(argv) != 7:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the intake pivot and integral intake pivot.'
+ )
+ else:
+ namespaces = [
+ 'y2024', 'control_loops', 'superstructure', 'intake_pivot'
+ ]
+ angular_system.WriteAngularSystem(kIntakePivot, argv[1:4], argv[4:7],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
\ No newline at end of file
diff --git a/y2024/control_loops/superstructure/intake_pivot/BUILD b/y2024/control_loops/superstructure/intake_pivot/BUILD
new file mode 100644
index 0000000..18c6963
--- /dev/null
+++ b/y2024/control_loops/superstructure/intake_pivot/BUILD
@@ -0,0 +1,42 @@
+package(default_visibility = ["//y2024:__subpackages__"])
+
+genrule(
+ name = "genrule_intake_pivot",
+ outs = [
+ "intake_pivot_plant.h",
+ "intake_pivot_plant.cc",
+ "intake_pivot_plant.json",
+ "integral_intake_pivot_plant.h",
+ "integral_intake_pivot_plant.cc",
+ "integral_intake_pivot_plant.json",
+ ],
+ cmd = "$(location //y2024/control_loops/python:intake_pivot) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2024/control_loops/python:intake_pivot",
+ ],
+)
+
+cc_library(
+ name = "intake_pivot_plants",
+ srcs = [
+ "intake_pivot_plant.cc",
+ "integral_intake_pivot_plant.cc",
+ ],
+ hdrs = [
+ "intake_pivot_plant.h",
+ "integral_intake_pivot_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 = "intake_pivot_json",
+ srcs = ["integral_intake_pivot_plant.json"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024/control_loops/superstructure/superstructure_can_position.fbs b/y2024/control_loops/superstructure/superstructure_can_position.fbs
index 04daa2b..46f638c 100644
--- a/y2024/control_loops/superstructure/superstructure_can_position.fbs
+++ b/y2024/control_loops/superstructure/superstructure_can_position.fbs
@@ -3,8 +3,20 @@
namespace y2024.control_loops.superstructure;
table CANPosition {
+ // The timestamp of the measurement on the canivore clock in nanoseconds
+ // This will have less jitter than the
+ // timestamp of the message being sent out.
+ timestamp:int64 (id: 0);
+
+ // The ctre::phoenix::StatusCode of the measurement
+ // Should be OK = 0
+ status:int (id: 1);
+
// CAN Position of the roller falcon
- intake_roller:frc971.control_loops.CANTalonFX (id: 0);
+ intake_roller:frc971.control_loops.CANTalonFX (id: 2);
+
+ // CAN Position of the intake pivot falcon
+ intake_pivot:frc971.control_loops.CANTalonFX (id: 3);
}
root_type CANPosition;
\ No newline at end of file