Add pivot joint codegen
Signed-off-by: Nathan Leong <100028864@mvla.net>
Change-Id: I8b035349e44430218623c4d0dc883c2b06a605c2
diff --git a/y2023_bot3/BUILD b/y2023_bot3/BUILD
index 6025e3f..c9258f0 100644
--- a/y2023_bot3/BUILD
+++ b/y2023_bot3/BUILD
@@ -156,7 +156,9 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:pot_and_absolute_encoder",
"//y2023_bot3/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2023_bot3/control_loops/superstructure/pivot_joint:pivot_joint_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
diff --git a/y2023_bot3/constants.cc b/y2023_bot3/constants.cc
index 909d508..9e1902c 100644
--- a/y2023_bot3/constants.cc
+++ b/y2023_bot3/constants.cc
@@ -12,6 +12,7 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
+#include "y2023_bot3/control_loops/superstructure/pivot_joint/integral_pivot_joint_plant.h"
namespace y2023_bot3 {
namespace constants {
@@ -20,9 +21,32 @@
LOG(INFO) << "creating a Constants for team: " << team;
Values r;
+ auto *const pivot_joint = &r.pivot_joint;
+
+ r.pivot_joint_flipped = true;
+
+ pivot_joint->subsystem_params.zeroing_voltage = 3.0;
+ pivot_joint->subsystem_params.operating_voltage = 12.0;
+ pivot_joint->subsystem_params.zeroing_profile_params = {0.5, 3.0};
+ pivot_joint->subsystem_params.default_profile_params = {0.5, 5.0};
+ pivot_joint->subsystem_params.range = Values::kPivotJointRange();
+ pivot_joint->subsystem_params.make_integral_loop =
+ control_loops::superstructure::pivot_joint::MakeIntegralPivotJointLoop;
+ pivot_joint->subsystem_params.zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ pivot_joint->subsystem_params.zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kPivotJointEncoderRatio();
+ pivot_joint->subsystem_params.zeroing_constants.zeroing_threshold = 0.0005;
+ pivot_joint->subsystem_params.zeroing_constants.moving_buffer_size = 20;
+ pivot_joint->subsystem_params.zeroing_constants.allowable_encoder_error = 0.9;
+
switch (team) {
// A set of constants for tests.
case 1:
+
+ pivot_joint->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+
break;
case kThirdRobotTeamNumber:
diff --git a/y2023_bot3/constants.h b/y2023_bot3/constants.h
index 6cb7d8e..fb3089e 100644
--- a/y2023_bot3/constants.h
+++ b/y2023_bot3/constants.h
@@ -1,5 +1,5 @@
-#ifndef Y2023_CONSTANTS_H_
-#define Y2023_CONSTANTS_H_
+#ifndef Y2023_BOT3_CONSTANTS_H_
+#define Y2023_BOT3_CONSTANTS_H_
#include <array>
#include <cmath>
@@ -8,8 +8,9 @@
#include "frc971/constants.h"
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2023_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-
+#include "y2023_bot3/control_loops/superstructure/pivot_joint/pivot_joint_plant.h"
namespace y2023_bot3 {
namespace constants {
@@ -60,6 +61,40 @@
control_loops::drivetrain::kHighOutputRatio *
control_loops::drivetrain::kWheelRadius;
}
+
+ struct PotAndAbsEncoderConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ subsystem_params;
+ double potentiometer_offset;
+ };
+
+ // Pivot Joint (placeholders)
+ static constexpr double kPivotJointEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+
+ static constexpr double kPivotJointEncoderRatio() { return 1.0; }
+
+ static constexpr double kMaxPivotJointEncoderPulsesPerSecond() {
+ return control_loops::superstructure::pivot_joint::kFreeSpeed /
+ (2.0 * M_PI) *
+ control_loops::superstructure::pivot_joint::kOutputRatio /
+ kPivotJointEncoderRatio() * kPivotJointEncoderCountsPerRevolution();
+ }
+
+ static constexpr ::frc971::constants::Range kPivotJointRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.10, // Back Hard
+ .upper_hard = 4.90, // Front Hard
+ .lower = 0.0, // Back Soft
+ .upper = 4.0, // Front Soft
+ };
+ }
+
+ PotAndAbsEncoderConstants pivot_joint;
+
+ bool pivot_joint_flipped;
};
// Creates and returns a Values instance for the constants.
@@ -73,4 +108,4 @@
} // namespace constants
} // namespace y2023_bot3
-#endif // Y2023_CONSTANTS_H_
+#endif // Y2023_BOT3_CONSTANTS_H_
diff --git a/y2023_bot3/constants/common.json b/y2023_bot3/constants/common.json
index 0967ef4..4a34526 100644
--- a/y2023_bot3/constants/common.json
+++ b/y2023_bot3/constants/common.json
@@ -1 +1,3 @@
-{}
+{
+ "dummy_value": 0
+}
diff --git a/y2023_bot3/constants/constants.fbs b/y2023_bot3/constants/constants.fbs
index 08f03be..f66e00f 100644
--- a/y2023_bot3/constants/constants.fbs
+++ b/y2023_bot3/constants/constants.fbs
@@ -1,6 +1,8 @@
namespace y2023_bot3;
table RobotConstants {
+ // Constants sender needs a value to send something
+ dummy_value:int (id: 0);
}
table Constants {
diff --git a/y2023_bot3/control_loops/python/BUILD b/y2023_bot3/control_loops/python/BUILD
index fe7666f..2753020 100644
--- a/y2023_bot3/control_loops/python/BUILD
+++ b/y2023_bot3/control_loops/python/BUILD
@@ -55,3 +55,19 @@
visibility = ["//visibility:public"],
deps = ["//y2023_bot3/control_loops:python_init"],
)
+
+py_binary(
+ name = "pivot_joint",
+ srcs = [
+ "pivot_joint.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",
+ ],
+)
diff --git a/y2023_bot3/control_loops/python/pivot_joint.py b/y2023_bot3/control_loops/python/pivot_joint.py
new file mode 100644
index 0000000..c2d94ba
--- /dev/null
+++ b/y2023_bot3/control_loops/python/pivot_joint.py
@@ -0,0 +1,59 @@
+#!/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
+
+kPivotJoint = angular_system.AngularSystemParams(
+ name='PivotJoint',
+ motor=control_loop.BAG(),
+ G=(6.0 / 48.0) * (20.0 / 100.0) * (18.0 / 24.0) * (24.0 / 44.0),
+ # Use parallel axis theorem to get the moment of inertia around
+ # the joint (I = I_cm + mh^2 = 0.001877 + 0.8332 * 0.0407162^2)
+ J=0.003258,
+ q_pos=0.80,
+ q_vel=80.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=0.5,
+ kalman_r_position=0.05,
+ radius=5.71 * 0.0254)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system.PlotKick(kPivotJoint, R)
+ angular_system.PlotMotion(kPivotJoint, R)
+ return
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the pivot joint and integral pivot joint.'
+ )
+ else:
+ namespaces = [
+ 'y2023_bot3', 'control_loops', 'superstructure', 'pivot_joint'
+ ]
+ angular_system.WriteAngularSystem(kPivotJoint, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD b/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD
new file mode 100644
index 0000000..ae413ab
--- /dev/null
+++ b/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD
@@ -0,0 +1,34 @@
+package(default_visibility = ["//y2023_bot3:__subpackages__"])
+
+genrule(
+ name = "genrule_pivot_joint",
+ outs = [
+ "pivot_joint_plant.h",
+ "pivot_joint_plant.cc",
+ "integral_pivot_joint_plant.h",
+ "integral_pivot_joint_plant.cc",
+ ],
+ cmd = "$(location //y2023_bot3/control_loops/python:pivot_joint) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2023_bot3/control_loops/python:pivot_joint",
+ ],
+)
+
+cc_library(
+ name = "pivot_joint_plants",
+ srcs = [
+ "integral_pivot_joint_plant.cc",
+ "pivot_joint_plant.cc",
+ ],
+ hdrs = [
+ "integral_pivot_joint_plant.h",
+ "pivot_joint_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",
+ ],
+)
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.cc b/y2023_bot3/control_loops/superstructure/superstructure.cc
index 4bc6a5d..3e88a6c 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure.cc
@@ -15,7 +15,6 @@
using ::aos::monotonic_clock;
-using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::RelativeEncoderProfiledJointStatus;
@@ -53,6 +52,7 @@
}
Status::Builder status_builder = status->MakeBuilder<Status>();
+
status_builder.add_zeroed(true);
status_builder.add_end_effector_state(end_effector_.state());
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.h b/y2023_bot3/control_loops/superstructure/superstructure.h
index 808ff3b..212ced8 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.h
+++ b/y2023_bot3/control_loops/superstructure/superstructure.h
@@ -22,6 +22,11 @@
class Superstructure
: public ::frc971::controls::ControlLoop<Goal, Position, Status, Output> {
public:
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
explicit Superstructure(::aos::EventLoop *event_loop,
std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");