Merge "Add code to train a simple turret controller" into main
diff --git a/documentation/tutorials/getting-started.md b/documentation/tutorials/getting-started.md
index 8e04fcb..fb8ddaf 100644
--- a/documentation/tutorials/getting-started.md
+++ b/documentation/tutorials/getting-started.md
@@ -80,7 +80,7 @@
## Running the Code
-To run the code you'll need to download the repository from [gerrit](https://software.frc971.org/gerrit/admin/repos/971-Robot-Code), make sure to select ssh and not http.
+To run the code you'll need to download the repository from [gerrit](https://software.frc971.org/gerrit/admin/repos/971-Robot-Code), make sure to select ssh and not http. Click on SSH, and clone with commit message hook. Copy the command, and run it locally on terminal.
To learn more about git, open a terminal and run `man git`, or see [git(1)](https://manpages.debian.org/buster/git-man/git.1.en.html) (especially the NOTES section).
Once the repositoy is selected you'll want to make sure to configure your name, email on git. This is required to ensure you're following the [contributing guidelines above](#contributing). You can do this by running these following commands:
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index 26828e4..1b8dcc3 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -67,7 +67,10 @@
":swerve_drivetrain_output_fbs",
":swerve_drivetrain_position_fbs",
":swerve_drivetrain_status_fbs",
+ ":swerve_zeroing_fbs",
"//frc971/control_loops:control_loop",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//frc971/zeroing:continuous_absolute_encoder",
],
)
@@ -76,6 +79,7 @@
srcs = ["swerve_control_test.cc"],
data = [
":aos_config",
+ "//frc971/control_loops/swerve/test_module:rotation_json",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
@@ -214,6 +218,7 @@
srcs = [
"physics_test.py",
],
+ data = [":cpp_dynamics.so"],
env = {
"JAX_PLATFORMS": "cpu",
},
@@ -234,6 +239,7 @@
srcs = [
"physics_test.py",
],
+ data = [":cpp_dynamics.so"],
env = {
"JAX_PLATFORMS": "cuda",
},
@@ -329,3 +335,18 @@
"@pip//pygobject",
],
)
+
+cc_binary(
+ name = "cpp_dynamics.so",
+ # Just use the python dynamics directly if you want them; this is just for testing.
+ testonly = True,
+ srcs = ["dynamics_python_bindings.cc"],
+ linkshared = True,
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":eigen_dynamics",
+ "//third_party/python",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
+ ],
+)
diff --git a/frc971/control_loops/swerve/casadi_velocity_mpc.py b/frc971/control_loops/swerve/casadi_velocity_mpc.py
index 239f49a..517de5a 100644
--- a/frc971/control_loops/swerve/casadi_velocity_mpc.py
+++ b/frc971/control_loops/swerve/casadi_velocity_mpc.py
@@ -47,16 +47,30 @@
# TODO(austin): Do we need a disturbance torque per module to account for friction?
# Do it only in the observer/post?
- self.next_X = self.make_physics()
- self.cost = self.make_cost()
self.force = [
dynamics.F(i, casadi.SX.sym("X", 25, 1), casadi.SX.sym("U", 8, 1))
for i in range(4)
]
+ self.force_vel = [
+ dynamics.F_vel(i,
+ casadi.SX.sym("X", dynamics.NUM_VELOCITY_STATES, 1),
+ casadi.SX.sym("U", 8, 1)) for i in range(4)
+ ]
self.slip_angle = [
dynamics.slip_angle(i, casadi.SX.sym("X", 25, 1),
casadi.SX.sym("U", 8, 1)) for i in range(4)
]
+ self.mounting_location = [
+ dynamics.mounting_location(
+ i, casadi.SX.sym("X", dynamics.NUM_VELOCITY_STATES, 1),
+ casadi.SX.sym("U", 8, 1)) for i in range(4)
+ ]
+ self.torque_cross = self.torque_cross_func(casadi.SX.sym("r", 2, 1),
+ casadi.SX.sym("F", 2, 1))
+ self.force_cross = self.force_cross_func(casadi.SX.sym("Tau", 1, 1),
+ casadi.SX.sym("r", 2, 1))
+ self.next_X = self.make_physics()
+ self.cost = self.make_cost()
self.N = 200
@@ -209,6 +223,8 @@
# TODO(austin): Do we want to do something more special for 0?
+ # cost velocity a lot more in the perpendicular direction to allow tire to spin up
+ # we also only want to get moving in the correct direction as fast as possible
J += 75 * ((R[0] - X[dynamics.VELOCITY_STATE_VX]) * vnormx +
(R[1] - X[dynamics.VELOCITY_STATE_VY]) * vnormy)**2.0
@@ -231,11 +247,29 @@
J += kSteerPositionGain * (X[dynamics.VELOCITY_STATE_THETAS3])**2.0
J += kSteerVelocityGain * (X[dynamics.VELOCITY_STATE_OMEGAS3])**2.0
+ # cost variance of the force by a tire and the expected average force and torque on it
+ total_force = self.force_vel[0](X, U)
+ total_torque = self.torque_cross(self.mounting_location[0](X, U),
+ self.force_vel[0](X, U))
+ for i in range(3):
+ total_force += self.force_vel[i + 1](X, U)
+ total_torque += self.torque_cross(
+ self.mounting_location[i + 1](X, U), self.force_vel[i + 1](X,
+ U))
+
+ total_force /= 4
+ total_torque /= 4
+ for i in range(4):
+ f_diff = (total_force +
+ self.force_cross(total_torque, self.mounting_location[i]
+ (X, U))) - self.force_vel[i](X, U)
+ J += 0.01 * (f_diff[0, 0]**2.0 + f_diff[1, 0]**2.0)
+
# TODO(austin): Don't penalize torque steering current.
for i in range(4):
Is = U[2 * i + 0]
Id = U[2 * i + 1]
- # Steer
+ # Steer, cost it a lot less than drive to be more agressive in steering
J += ((Is + dynamics.STEER_CURRENT_COUPLING_FACTOR * Id)**
2.0) / 100000.0
# Drive
@@ -243,6 +277,17 @@
return casadi.Function("Jn", [X, U, R], [J])
+ def torque_cross_func(self, r, F):
+ result = casadi.SX.sym('Tau', 1, 1)
+ result[0, 0] = r[0, 0] * F[1, 0] - r[1, 0] * F[0, 0]
+ return casadi.Function('Tau', [r, F], [result])
+
+ def force_cross_func(self, Tau, r):
+ result = casadi.SX.sym('F', 2, 1)
+ result[0, 0] = -r[1, 0] * Tau[0, 0] / casadi.norm_2(r)**2.0
+ result[1, 0] = r[0, 0] * Tau[0, 0] / casadi.norm_2(r)**2.0
+ return casadi.Function('F', [Tau, r], [result])
+
def solve(self, p, seed=None):
if seed is None:
seed = []
@@ -334,6 +379,9 @@
self.X_plot[dynamics.STATE_VX, 0:i + 1],
label="vx")
axs0[0].plot(self.t,
+ self.X_plot[dynamics.STATE_OMEGA, 0:i + 1],
+ label="omega")
+ axs0[0].plot(self.t,
self.X_plot[dynamics.STATE_VY, 0:i + 1],
label="vy")
axs0[0].legend()
diff --git a/frc971/control_loops/swerve/dynamics_python_bindings.cc b/frc971/control_loops/swerve/dynamics_python_bindings.cc
new file mode 100644
index 0000000..e1fbe19
--- /dev/null
+++ b/frc971/control_loops/swerve/dynamics_python_bindings.cc
@@ -0,0 +1,103 @@
+#define PY_SSIZE_T_CLEAN
+// Note that Python.h needs to be included before anything else.
+#include <Python.h>
+
+#include <iostream>
+#include <optional>
+
+#include "frc971/control_loops/swerve/dynamics.h"
+
+namespace frc971::control_loops::swerve {
+namespace {
+template <int N>
+std::optional<Eigen::Matrix<double, N, 1>> ToEigen(PyObject *list) {
+ Eigen::Matrix<double, N, 1> result;
+ for (size_t index = 0; index < N; ++index) {
+ PyObject *element = PyList_GetItem(list, index);
+ if (!PyFloat_Check(element)) {
+ PyErr_SetString(PyExc_ValueError,
+ "Input lists should be lists of floats.");
+ return std::nullopt;
+ }
+ result(index) = PyFloat_AsDouble(element);
+ }
+ return result;
+}
+PyObject *swerve_dynamics(PyObject * /*self*/, PyObject *args) {
+ PyObject *X;
+ PyObject *U;
+ if (!PyArg_ParseTuple(args, "OO", &X, &U)) {
+ PyErr_SetString(PyExc_ValueError, "Input arguments should be two lists.");
+ return nullptr;
+ }
+
+ if (!PyList_Check(X)) {
+ PyErr_SetString(PyExc_ValueError, "X should be a list.");
+ return nullptr;
+ }
+ if (!PyList_Check(U)) {
+ PyErr_SetString(PyExc_ValueError, "U should be a list.");
+ return nullptr;
+ }
+
+ if (PyList_Size(X) != kNumFullDynamicsStates) {
+ PyErr_SetString(PyExc_ValueError,
+ "X should have kNumFullDynamicsStates elements.");
+ return nullptr;
+ }
+
+ if (PyList_Size(U) != kNumInputs) {
+ PyErr_SetString(PyExc_ValueError, "U should have kNumInputs elements.");
+ return nullptr;
+ }
+
+ std::optional<Eigen::Matrix<double, kNumFullDynamicsStates, 1>> X_eig =
+ ToEigen<kNumFullDynamicsStates>(X);
+
+ if (!X_eig.has_value()) {
+ return nullptr;
+ }
+
+ std::optional<Eigen::Matrix<double, kNumInputs, 1>> U_eig =
+ ToEigen<kNumInputs>(X);
+
+ if (!U_eig.has_value()) {
+ return nullptr;
+ }
+
+ Eigen::Matrix<double, kNumFullDynamicsStates, 1> Xdot =
+ SwervePhysics(X_eig.value(), U_eig.value());
+
+ PyObject *result = PyList_New(kNumFullDynamicsStates);
+ for (size_t index = 0; index < kNumFullDynamicsStates; ++index) {
+ if (PyList_SetItem(result, index, PyFloat_FromDouble(Xdot(index))) != 0) {
+ return nullptr;
+ }
+ }
+
+ return result;
+}
+
+static PyMethodDef methods[] = {
+ {"swerve_dynamics", swerve_dynamics, METH_VARARGS,
+ "Xdot = swerve_dynamics(X, U), all types are lists."},
+ {NULL, NULL, 0, NULL} // Sentinel
+};
+
+static PyModuleDef cpp_dynamics_module = {
+ .m_base = PyModuleDef_HEAD_INIT,
+ .m_name = "cpp_dynamics",
+ .m_doc =
+ "Wraps the generated C++ dynamics in order to support convenient "
+ "testing.",
+ .m_size = -1,
+ .m_methods = methods,
+};
+
+PyObject *InitModule() { return PyModule_Create(&cpp_dynamics_module); }
+} // namespace
+} // namespace frc971::control_loops::swerve
+
+PyMODINIT_FUNC PyInit_cpp_dynamics(void) {
+ return frc971::control_loops::swerve::InitModule();
+}
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index 748e3cb..af058f6 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -283,18 +283,107 @@
result_h.emplace_back("");
result_h.emplace_back("namespace frc971::control_loops::swerve {");
result_h.emplace_back("");
+ result_h.emplace_back("struct FullDynamicsStates {");
+ result_h.emplace_back("enum States {");
+ result_h.emplace_back(" kThetas0 = 0,");
+ result_h.emplace_back(" kThetad0 = 1,");
+ result_h.emplace_back(" kOmegas0 = 2,");
+ result_h.emplace_back(" kOmegad0 = 3,");
+ result_h.emplace_back(" kThetas1 = 4,");
+ result_h.emplace_back(" kThetad1 = 5,");
+ result_h.emplace_back(" kOmegas1 = 6,");
+ result_h.emplace_back(" kOmegad1 = 7,");
+ result_h.emplace_back(" kThetas2 = 8,");
+ result_h.emplace_back(" kThetad2 = 9,");
+ result_h.emplace_back(" kOmegas2 = 10,");
+ result_h.emplace_back(" kOmegad2 = 11,");
+ result_h.emplace_back(" kThetas3 = 12,");
+ result_h.emplace_back(" kThetad3 = 13,");
+ result_h.emplace_back(" kOmegas3 = 14,");
+ result_h.emplace_back(" kOmegad3 = 15,");
+ result_h.emplace_back(" kX = 16,");
+ result_h.emplace_back(" kY = 17,");
+ result_h.emplace_back(" kTheta = 18,");
+ result_h.emplace_back(" kVx = 19,");
+ result_h.emplace_back(" kVy = 20,");
+ result_h.emplace_back(" kOmega = 21,");
+ result_h.emplace_back(" kFx = 22,");
+ result_h.emplace_back(" kFy = 23,");
+ result_h.emplace_back(" kMoment = 24,");
+ result_h.emplace_back(" kNumStates");
+ result_h.emplace_back("};");
+ result_h.emplace_back("};");
+ result_h.emplace_back(
+ "inline constexpr size_t kNumFullDynamicsStates = "
+ "static_cast<size_t>(FullDynamicsStates::kNumStates);");
+ result_h.emplace_back("struct VelocityStates {");
+ result_h.emplace_back("enum States {");
+ result_h.emplace_back(" kThetas0 = 0,");
+ result_h.emplace_back(" kOmegas0 = 1,");
+ result_h.emplace_back(" kThetas1 = 2,");
+ result_h.emplace_back(" kOmegas1 = 3,");
+ result_h.emplace_back(" kThetas2 = 4,");
+ result_h.emplace_back(" kOmegas2 = 5,");
+ result_h.emplace_back(" kThetas3 = 6,");
+ result_h.emplace_back(" kOmegas3 = 7,");
+ result_h.emplace_back(" kTheta = 8,");
+ result_h.emplace_back(" kVx = 9,");
+ result_h.emplace_back(" kVy = 10,");
+ result_h.emplace_back(" kOmega = 11,");
+ result_h.emplace_back(" kNumStates");
+ result_h.emplace_back("};");
+ result_h.emplace_back("};");
+ result_h.emplace_back(
+ "inline constexpr size_t kNumVelocityStates = "
+ "static_cast<size_t>(VelocityStates::kNumStates);");
+ result_h.emplace_back("struct Inputs {");
+ result_h.emplace_back("enum States {");
+ result_h.emplace_back(" kIs0 = 0,");
+ result_h.emplace_back(" kId0 = 1,");
+ result_h.emplace_back(" kIs1 = 2,");
+ result_h.emplace_back(" kId1 = 3,");
+ result_h.emplace_back(" kIs2 = 4,");
+ result_h.emplace_back(" kId2 = 5,");
+ result_h.emplace_back(" kIs3 = 6,");
+ result_h.emplace_back(" kId3 = 7,");
+ result_h.emplace_back(" kNumInputs = 8");
+ result_h.emplace_back("};");
+ result_h.emplace_back("};");
+ result_h.emplace_back(
+ "inline constexpr size_t kNumInputs = "
+ "static_cast<size_t>(Inputs::kNumInputs);");
+ result_h.emplace_back("");
result_h.emplace_back("// Returns the derivative of our state vector");
- result_h.emplace_back("// [thetas0, thetad0, omegas0, omegad0,");
- result_h.emplace_back("// thetas1, thetad1, omegas1, omegad1,");
- result_h.emplace_back("// thetas2, thetad2, omegas2, omegad2,");
- result_h.emplace_back("// thetas3, thetad3, omegas3, omegad3,");
- result_h.emplace_back("// x, y, theta, vx, vy, omega,");
- result_h.emplace_back("// Fx, Fy, Moment]");
- result_h.emplace_back("Eigen::Matrix<double, 25, 1> SwervePhysics(");
result_h.emplace_back(
- " Eigen::Map<const Eigen::Matrix<double, 25, 1>> X,");
+ "Eigen::Matrix<double, kNumFullDynamicsStates, 1> SwervePhysics(");
result_h.emplace_back(
- " Eigen::Map<const Eigen::Matrix<double, 8, 1>> U);");
+ " Eigen::Ref<const Eigen::Matrix<double, kNumFullDynamicsStates, "
+ "1>> X,");
+ result_h.emplace_back(
+ " Eigen::Ref<const Eigen::Matrix<double, kNumInputs, 1>> U);");
+ result_h.emplace_back("");
+ result_h.emplace_back(
+ "Eigen::Matrix<double, kNumVelocityStates, 1> ToVelocityState(");
+ result_h.emplace_back(
+ " Eigen::Ref<const Eigen::Matrix<double, kNumFullDynamicsStates, "
+ "1>> X);");
+ result_h.emplace_back("");
+ result_h.emplace_back(
+ "Eigen::Matrix<double, kNumFullDynamicsStates, 1> FromVelocityState(");
+ result_h.emplace_back(
+ " Eigen::Ref<const Eigen::Matrix<double, kNumVelocityStates, 1>> "
+ "X);");
+ result_h.emplace_back("");
+ result_h.emplace_back(
+ "inline Eigen::Matrix<double, kNumVelocityStates, 1> VelocityPhysics(");
+ result_h.emplace_back(
+ " Eigen::Ref<const Eigen::Matrix<double, kNumVelocityStates, 1>> "
+ "X,");
+ result_h.emplace_back(
+ " Eigen::Ref<const Eigen::Matrix<double, kNumInputs, 1>> U) {");
+ result_h.emplace_back(
+ " return ToVelocityState(SwervePhysics(FromVelocityState(X), U));");
+ result_h.emplace_back("}");
result_h.emplace_back("");
result_h.emplace_back("} // namespace frc971::control_loops::swerve");
result_h.emplace_back("");
@@ -308,12 +397,49 @@
result_cc.emplace_back("");
result_cc.emplace_back("namespace frc971::control_loops::swerve {");
result_cc.emplace_back("");
- result_cc.emplace_back("Eigen::Matrix<double, 25, 1> SwervePhysics(");
result_cc.emplace_back(
- " Eigen::Map<const Eigen::Matrix<double, 25, 1>> X,");
+ "Eigen::Matrix<double, kNumVelocityStates, 1> ToVelocityState(");
result_cc.emplace_back(
- " Eigen::Map<const Eigen::Matrix<double, 8, 1>> U) {");
- result_cc.emplace_back(" Eigen::Matrix<double, 25, 1> result;");
+ " Eigen::Ref<const Eigen::Matrix<double, kNumFullDynamicsStates, "
+ "1>> X) {");
+ result_cc.emplace_back(
+ " Eigen::Matrix<double, kNumVelocityStates, 1> velocity;");
+ const std::vector<std::string_view> velocity_states = {
+ "kThetas0", "kOmegas0", "kThetas1", "kOmegas1", "kThetas2", "kOmegas2",
+ "kThetas3", "kOmegas3", "kTheta", "kVx", "kVy", "kOmega"};
+ for (const std::string_view velocity_state : velocity_states) {
+ result_cc.emplace_back(absl::StrFormat(
+ " velocity(VelocityStates::%s) = X(FullDynamicsStates::%s);",
+ velocity_state, velocity_state));
+ }
+ result_cc.emplace_back(" return velocity;");
+ result_cc.emplace_back("}");
+ result_cc.emplace_back("");
+ result_cc.emplace_back(
+ "Eigen::Matrix<double, kNumFullDynamicsStates, 1> FromVelocityState(");
+ result_cc.emplace_back(
+ " Eigen::Ref<const Eigen::Matrix<double, kNumVelocityStates, 1>> X) "
+ "{");
+ result_cc.emplace_back(
+ " Eigen::Matrix<double, kNumFullDynamicsStates, 1> full;");
+ result_cc.emplace_back(" full.setZero();");
+ for (const std::string_view velocity_state : velocity_states) {
+ result_cc.emplace_back(absl::StrFormat(
+ " full(FullDynamicsStates::%s) = X(VelocityStates::%s);",
+ velocity_state, velocity_state));
+ }
+ result_cc.emplace_back(" return full;");
+ result_cc.emplace_back("}");
+ result_cc.emplace_back("");
+ result_cc.emplace_back(
+ "Eigen::Matrix<double, kNumFullDynamicsStates, 1> SwervePhysics(");
+ result_cc.emplace_back(
+ " Eigen::Ref<const Eigen::Matrix<double, kNumFullDynamicsStates, "
+ "1>> X,");
+ result_cc.emplace_back(
+ " Eigen::Ref<const Eigen::Matrix<double, kNumInputs, 1>> U) {");
+ result_cc.emplace_back(
+ " Eigen::Matrix<double, kNumFullDynamicsStates, 1> result;");
// Start by writing out variables matching each of the symbol names we use
// so we don't have to modify the computed equations too much.
@@ -785,7 +911,16 @@
},
&result_py);
- DefineVector2dFunction(
+ DefineVector2dVelocityFunction(
+ "F_vel",
+ "Returns the force on the wheel in absolute coordinates based on the "
+ "velocity controller",
+ [](const Module &m, int dimension) {
+ return ccode(*m.direct.F.get(dimension, 0));
+ },
+ &result_py);
+
+ DefineVector2dVelocityFunction(
"mounting_location",
"Returns the mounting location of wheel in robot coordinates",
[](const Module &m, int dimension) {
@@ -852,6 +987,34 @@
" return casadi.Function('$0', [X, U], [result])", name));
}
+ void DefineVector2dVelocityFunction(
+ std::string_view name, std::string_view documentation,
+ std::function<std::string(const Module &, int)> scalar_fn,
+ std::vector<std::string> *result_py) {
+ result_py->emplace_back("");
+ result_py->emplace_back(absl::Substitute("# $0.", documentation));
+ result_py->emplace_back(absl::Substitute("def $0(i, X, U):", name));
+ WriteCasadiVelocityVariables(result_py);
+ result_py->emplace_back(
+ absl::Substitute(" result = casadi.SX.sym('$0', 2, 1)", name));
+ for (size_t m = 0; m < kNumModules; ++m) {
+ if (m == 0) {
+ result_py->emplace_back(" if i == 0:");
+ } else {
+ result_py->emplace_back(absl::Substitute(" elif i == $0:", m));
+ }
+ for (int j = 0; j < 2; ++j) {
+ result_py->emplace_back(absl::Substitute(" result[$0, 0] = $1",
+ j, scalar_fn(modules_[m], j)));
+ }
+ }
+ result_py->emplace_back(" else:");
+ result_py->emplace_back(
+ " raise ValueError(\"Invalid module number\")");
+ result_py->emplace_back(absl::Substitute(
+ " return casadi.Function('$0', [X, U], [result])", name));
+ }
+
private:
static constexpr uint8_t kNumModules = 4;
diff --git a/frc971/control_loops/swerve/physics_test.py b/frc971/control_loops/swerve/physics_test.py
index a45bf46..1da2a31 100644
--- a/frc971/control_loops/swerve/physics_test.py
+++ b/frc971/control_loops/swerve/physics_test.py
@@ -19,6 +19,7 @@
from frc971.control_loops.swerve import nocaster_dynamics
from frc971.control_loops.swerve import physics_test_utils as utils
from frc971.control_loops.swerve import jax_dynamics
+from frc971.control_loops.swerve.cpp_dynamics import swerve_dynamics as cpp_dynamics
class TestSwervePhysics(unittest.TestCase):
@@ -709,6 +710,29 @@
self.assertAlmostEquals(Xdot[dynamics.STATE_OMEGA, 0],
Xdot_rot[dynamics.STATE_OMEGA, 0])
+ def test_cpp_consistency(self):
+ """Tests that the C++ physics are consistent with the Python physics."""
+ # TODO(james): Currently the physics only match at X = 0 and U = 0.
+ # Fix this.
+ # Maybe due to different atan2 implementations?
+ # TODO(james): Fold this into the general comparisons for JAX versus
+ # casadi once the physics actually match.
+ for current in [0]:
+ print(f"Current: {current}")
+ steer_I = numpy.zeros((8, 1)) + current
+ for state_values in [0.0]:
+ print(f"States all set to: {state_values}")
+ X = numpy.zeros((dynamics.NUM_STATES, 1)) + state_values
+ Xdot_py = self.swerve_full_dynamics(X,
+ steer_I,
+ skip_compare=True)
+ Xdot_cpp = numpy.array(
+ cpp_dynamics(X.flatten().tolist(),
+ steer_I.flatten().tolist())).reshape((25, 1))
+ for index in range(dynamics.NUM_STATES):
+ self.assertAlmostEqual(Xdot_py[index, 0], Xdot_cpp[index,
+ 0])
+
if __name__ == "__main__":
unittest.main()
diff --git a/frc971/control_loops/swerve/swerve_control_loops.cc b/frc971/control_loops/swerve/swerve_control_loops.cc
index bd0d3bd..ec841c8 100644
--- a/frc971/control_loops/swerve/swerve_control_loops.cc
+++ b/frc971/control_loops/swerve/swerve_control_loops.cc
@@ -2,39 +2,114 @@
namespace frc971::control_loops::swerve {
-SwerveControlLoops::SwerveControlLoops(::aos::EventLoop *event_loop,
- const ::std::string &name)
+SwerveControlLoops::SwerveControlLoops(
+ ::aos::EventLoop *event_loop,
+ const frc971::control_loops::
+ StaticZeroingSingleDOFProfiledSubsystemCommonParams *rotation_params,
+ const SwerveZeroing *zeroing_params, const ::std::string &name)
: frc971::controls::ControlLoop<Goal, Position, StatusStatic, OutputStatic>(
- event_loop, name) {}
+ event_loop, name),
+ front_left_(rotation_params, zeroing_params->front_left()),
+ front_right_(rotation_params, zeroing_params->front_right()),
+ back_left_(rotation_params, zeroing_params->back_left()),
+ back_right_(rotation_params, zeroing_params->back_right()) {}
void SwerveControlLoops::RunIteration(
const Goal *goal, const Position *position,
aos::Sender<OutputStatic>::StaticBuilder *output_builder,
aos::Sender<StatusStatic>::StaticBuilder *status_builder) {
- (void)goal, (void)position;
+ if (WasReset()) {
+ front_left_.Reset();
+ front_right_.Reset();
+ back_left_.Reset();
+ back_right_.Reset();
+ }
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
+ front_left_goal_buffer, front_right_goal_buffer, back_left_goal_buffer,
+ back_right_goal_buffer;
- if (output_builder != nullptr && goal != nullptr) {
+ front_left_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *front_left_goal_buffer.fbb(),
+ (goal != nullptr && goal->has_front_left_goal())
+ ? goal->front_left_goal()->rotation_angle()
+ : 0.0));
+ front_right_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *front_right_goal_buffer.fbb(),
+ (goal != nullptr && goal->has_front_right_goal())
+ ? goal->front_right_goal()->rotation_angle()
+ : 0.0));
+ back_left_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *back_left_goal_buffer.fbb(),
+ (goal != nullptr && goal->has_back_left_goal())
+ ? goal->back_left_goal()->rotation_angle()
+ : 0.0));
+ back_right_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *back_right_goal_buffer.fbb(),
+ (goal != nullptr && goal->has_back_right_goal())
+ ? goal->back_right_goal()->rotation_angle()
+ : 0.0));
+ const bool disabled = front_left_.Correct(
+ &front_left_goal_buffer.message(),
+ position->front_left()->rotation_position(), output_builder == nullptr);
+ front_right_.Correct(&front_right_goal_buffer.message(),
+ position->front_right()->rotation_position(),
+ output_builder == nullptr);
+ back_left_.Correct(&back_left_goal_buffer.message(),
+ position->back_left()->rotation_position(),
+ output_builder == nullptr);
+ back_right_.Correct(&back_right_goal_buffer.message(),
+ position->back_right()->rotation_position(),
+ output_builder == nullptr);
+
+ const double front_left_voltage = front_left_.UpdateController(disabled);
+ front_left_.UpdateObserver(front_left_voltage);
+ const double front_right_voltage = front_right_.UpdateController(disabled);
+ front_right_.UpdateObserver(front_right_voltage);
+ const double back_left_voltage = back_left_.UpdateController(disabled);
+ back_left_.UpdateObserver(back_left_voltage);
+ const double back_right_voltage = back_right_.UpdateController(disabled);
+ back_right_.UpdateObserver(back_right_voltage);
+ (void)goal, (void)position;
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::AbsoluteEncoderProfiledJointStatus, 512>
+ front_left_status_buffer, front_right_status_buffer,
+ back_left_status_buffer, back_right_status_buffer;
+ front_left_status_buffer.Finish(
+ front_left_.MakeStatus(front_left_status_buffer.fbb()));
+ front_right_status_buffer.Finish(
+ front_right_.MakeStatus(front_right_status_buffer.fbb()));
+ back_left_status_buffer.Finish(
+ back_left_.MakeStatus(back_left_status_buffer.fbb()));
+ back_right_status_buffer.Finish(
+ back_right_.MakeStatus(back_right_status_buffer.fbb()));
+
+ if (output_builder != nullptr) {
OutputStatic *output = output_builder->get();
auto front_left_output = output->add_front_left_output();
- front_left_output->set_rotation_current(0);
+ front_left_output->set_rotation_current(front_left_voltage);
front_left_output->set_translation_current(
- goal->front_left_goal()->translation_current());
+ goal ? goal->front_left_goal()->translation_current() : 0.0);
auto front_right_output = output->add_front_right_output();
- front_right_output->set_rotation_current(0);
+ front_right_output->set_rotation_current(front_right_voltage);
front_right_output->set_translation_current(
- goal->front_right_goal()->translation_current());
+ goal ? goal->front_right_goal()->translation_current() : 0.0);
auto back_left_output = output->add_back_left_output();
- back_left_output->set_rotation_current(0);
+ back_left_output->set_rotation_current(back_left_voltage);
back_left_output->set_translation_current(
- goal->back_left_goal()->translation_current());
+ goal ? goal->back_left_goal()->translation_current() : 0.0);
auto back_right_output = output->add_back_right_output();
- back_right_output->set_rotation_current(0);
+ back_right_output->set_rotation_current(back_right_voltage);
back_right_output->set_translation_current(
- goal->back_right_goal()->translation_current());
+ goal ? goal->back_right_goal()->translation_current() : 0.0);
// Ignore the return value of Send
output_builder->CheckOk(output_builder->Send());
@@ -44,16 +119,20 @@
StatusStatic *status = status_builder->get();
auto front_left_status = status->add_front_left_status();
- PopulateSwerveModuleRotation(front_left_status);
+ PopulateSwerveModuleRotation(front_left_status,
+ &front_left_status_buffer.message());
auto front_right_status = status->add_front_right_status();
- PopulateSwerveModuleRotation(front_right_status);
+ PopulateSwerveModuleRotation(front_right_status,
+ &front_right_status_buffer.message());
auto back_left_status = status->add_back_left_status();
- PopulateSwerveModuleRotation(back_left_status);
+ PopulateSwerveModuleRotation(back_left_status,
+ &back_left_status_buffer.message());
auto back_right_status = status->add_back_right_status();
- PopulateSwerveModuleRotation(back_right_status);
+ PopulateSwerveModuleRotation(back_right_status,
+ &back_right_status_buffer.message());
// Ignore the return value of Send
status_builder->CheckOk(status_builder->Send());
diff --git a/frc971/control_loops/swerve/swerve_control_loops.h b/frc971/control_loops/swerve/swerve_control_loops.h
index 78f6ba0..68016ab 100644
--- a/frc971/control_loops/swerve/swerve_control_loops.h
+++ b/frc971/control_loops/swerve/swerve_control_loops.h
@@ -4,19 +4,24 @@
#include "aos/time/time.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/control_loops/profiled_subsystem_static.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_can_position_generated.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_goal_generated.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_output_static.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_position_generated.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_status_static.h"
+#include "frc971/control_loops/swerve/swerve_zeroing_static.h"
+#include "frc971/zeroing/continuous_absolute_encoder.h"
namespace frc971::control_loops::swerve {
inline void PopulateSwerveModuleRotation(
- SwerveModuleStatusStatic *swerve_module_table) {
+ SwerveModuleStatusStatic *swerve_module_table,
+ const frc971::control_loops::AbsoluteEncoderProfiledJointStatus
+ *rotation_status) {
auto rotation = swerve_module_table->add_rotation();
- auto estimator_state = rotation->add_estimator_state();
- (void)estimator_state;
+ CHECK(rotation->FromFlatbuffer(rotation_status));
}
// Handles the translation and rotation current for each swerve module
@@ -24,14 +29,24 @@
: public ::frc971::controls::ControlLoop<Goal, Position, StatusStatic,
OutputStatic> {
public:
- explicit SwerveControlLoops(::aos::EventLoop *event_loop,
- const ::std::string &name = "/swerve");
+ using AbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::ContinuousAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
+ explicit SwerveControlLoops(
+ ::aos::EventLoop *event_loop,
+ const frc971::control_loops::
+ StaticZeroingSingleDOFProfiledSubsystemCommonParams *rotation_params,
+ const SwerveZeroing *zeroing_params,
+ const ::std::string &name = "/swerve");
protected:
void RunIteration(
const Goal *goal, const Position *position,
aos::Sender<OutputStatic>::StaticBuilder *output_builder,
aos::Sender<StatusStatic>::StaticBuilder *status_builder) override;
+ AbsoluteEncoderSubsystem front_left_, front_right_, back_left_, back_right_;
};
} // namespace frc971::control_loops::swerve
diff --git a/frc971/control_loops/swerve/swerve_control_test.cc b/frc971/control_loops/swerve/swerve_control_test.cc
index 9378f1c..051d1a7 100644
--- a/frc971/control_loops/swerve/swerve_control_test.cc
+++ b/frc971/control_loops/swerve/swerve_control_test.cc
@@ -4,15 +4,16 @@
#include <memory>
#include <vector>
+#include "absl/strings/str_format.h"
#include "gtest/gtest.h"
#include "aos/events/shm_event_loop.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/swerve/swerve_control_loops.h"
-#include "frc971/control_loops/swerve/swerve_drivetrain_can_position_generated.h"
-#include "frc971/control_loops/swerve/swerve_drivetrain_goal_generated.h"
+#include "frc971/control_loops/swerve/swerve_drivetrain_can_position_static.h"
+#include "frc971/control_loops/swerve/swerve_drivetrain_goal_static.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_output_generated.h"
-#include "frc971/control_loops/swerve/swerve_drivetrain_position_generated.h"
+#include "frc971/control_loops/swerve/swerve_drivetrain_position_static.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_status_generated.h"
#include "frc971/control_loops/team_number_test_environment.h"
@@ -27,7 +28,7 @@
public:
SwerveControlSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
: event_loop_(event_loop),
- position_sender_(event_loop_->MakeSender<Position>("/swerve")),
+ position_sender_(event_loop_->MakeSender<PositionStatic>("/swerve")),
can_position_sender_(event_loop_->MakeSender<CanPosition>("/swerve")),
goal_fetcher_(event_loop_->MakeFetcher<Goal>("/swerve")),
status_fetcher_(event_loop_->MakeFetcher<Status>("/swerve")),
@@ -45,12 +46,14 @@
// Sends a queue message with the position data.
void SendPositionMessage() {
- auto builder = position_sender_.MakeBuilder();
+ auto builder = position_sender_.MakeStaticBuilder();
- Position::Builder position_builder = builder.MakeBuilder<Position>();
+ builder->add_front_left()->add_rotation_position();
+ builder->add_front_right()->add_rotation_position();
+ builder->add_back_left()->add_rotation_position();
+ builder->add_back_right()->add_rotation_position();
- EXPECT_EQ(builder.Send(position_builder.Finish()),
- aos::RawSender::Error::kOk);
+ EXPECT_EQ(builder.Send(), aos::RawSender::Error::kOk);
}
void VerifyNearGoal() {
@@ -88,7 +91,7 @@
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<Position> position_sender_;
+ ::aos::Sender<PositionStatic> position_sender_;
::aos::Sender<CanPosition> can_position_sender_;
::aos::Sender<Goal> goal_sender_;
@@ -112,7 +115,57 @@
goal_sender_(swerve_test_event_loop_->MakeSender<Goal>("/swerve")),
swerve_control_event_loop_(MakeEventLoop("swerve_control")),
- swerve_control_loops_(swerve_control_event_loop_.get(), "/swerve"),
+ subsystem_params_(
+ aos::JsonToFlatbuffer<
+ frc971::control_loops::
+ StaticZeroingSingleDOFProfiledSubsystemCommonParams>(
+ absl::StrFormat(R"json({
+ "zeroing_voltage": 3.0,
+ "operating_voltage": 12.0,
+ "zeroing_profile_params": {
+ "max_velocity": 0.5,
+ "max_acceleration": 3.0
+ },
+ "default_profile_params":{
+ "max_velocity": 12.0,
+ "max_acceleration": 55.0
+ },
+ "range": {
+ "lower_hard": -inf,
+ "upper_hard": inf,
+ "lower": -inf,
+ "upper": inf
+ },
+ "loop": %s
+ })json",
+ aos::util::ReadFileToStringOrDie(
+ "frc971/control_loops/swerve/test_module/"
+ "integral_rotation_plant.json")))),
+ zeroing_params_(aos::JsonToFlatbuffer<SwerveZeroing>(R"json({
+ "front_left": {
+ "average_filter_size": 10,
+ "one_revolution_distance": 6,
+ "moving_buffer_size": 10
+ },
+ "front_right": {
+ "average_filter_size": 10,
+ "one_revolution_distance": 6,
+ "moving_buffer_size": 10
+ },
+ "back_left": {
+ "average_filter_size": 10,
+ "one_revolution_distance": 6,
+ "moving_buffer_size": 10
+ },
+ "back_right": {
+ "average_filter_size": 10,
+ "one_revolution_distance": 6,
+ "moving_buffer_size": 10
+ }
+ })json")),
+ swerve_control_loops_(swerve_control_event_loop_.get(),
+ &subsystem_params_.message(),
+ &zeroing_params_.message(), "/swerve"),
swerve_control_simulation_event_loop_(MakeEventLoop("simulation")),
swerve_control_simulation_(swerve_control_simulation_event_loop_.get(),
@@ -125,6 +178,11 @@
::aos::Sender<Goal> goal_sender_;
::std::unique_ptr<::aos::EventLoop> swerve_control_event_loop_;
+ aos::FlatbufferDetachedBuffer<
+ frc971::control_loops::
+ StaticZeroingSingleDOFProfiledSubsystemCommonParams>
+ subsystem_params_;
+ aos::FlatbufferDetachedBuffer<SwerveZeroing> zeroing_params_;
SwerveControlLoops swerve_control_loops_;
::std::unique_ptr<::aos::EventLoop> swerve_control_simulation_event_loop_;
@@ -158,4 +216,4 @@
swerve_control_simulation_.VerifyNearGoal();
}
-} // namespace frc971::control_loops::swerve::testing
\ No newline at end of file
+} // namespace frc971::control_loops::swerve::testing
diff --git a/frc971/control_loops/swerve/test_module/BUILD b/frc971/control_loops/swerve/test_module/BUILD
new file mode 100644
index 0000000..972bddf
--- /dev/null
+++ b/frc971/control_loops/swerve/test_module/BUILD
@@ -0,0 +1,36 @@
+py_binary(
+ name = "rotation",
+ srcs = [
+ "rotation.py",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ "//frc971/control_loops/python:angular_system_current",
+ "//frc971/control_loops/python:controls",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
+
+genrule(
+ name = "genrule_rotation",
+ outs = [
+ "rotation_plant.h",
+ "rotation_plant.cc",
+ "rotation_plant.json",
+ "integral_rotation_plant.h",
+ "integral_rotation_plant.cc",
+ "integral_rotation_plant.json",
+ ],
+ cmd = "$(location :rotation) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ ":rotation",
+ ],
+)
+
+filegroup(
+ name = "rotation_json",
+ srcs = ["integral_rotation_plant.json"],
+ visibility = ["//visibility:public"],
+)
diff --git a/frc971/control_loops/swerve/test_module/rotation.py b/frc971/control_loops/swerve/test_module/rotation.py
new file mode 100644
index 0000000..3f94e53
--- /dev/null
+++ b/frc971/control_loops/swerve/test_module/rotation.py
@@ -0,0 +1,57 @@
+#!/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_current
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+import matplotlib
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kRotation = angular_system_current.AngularSystemCurrentParams(
+ name='Rotation',
+ motor=control_loop.KrakenFOC(),
+ G=9.0 / 24.0 * 14.0 / 72.0,
+ J=3.1 / 1000.0,
+ q_pos=0.05,
+ 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=25 * 0.0254,
+ wrap_point=2.0 * numpy.pi)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system_current.PlotKick(kRotation, R)
+ angular_system_current.PlotMotion(kRotation, 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 wrist and integral wrist.'
+ )
+ else:
+ namespaces = ['frc971', 'control_loops', 'swerve', 'test_module']
+ angular_system_current.WriteAngularSystemCurrent(
+ kRotation, argv[1:4], argv[4:7], namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2024_swerve/BUILD b/y2024_swerve/BUILD
index e432358..2357c6e 100644
--- a/y2024_swerve/BUILD
+++ b/y2024_swerve/BUILD
@@ -32,6 +32,7 @@
"//aos/starter:irq_affinity",
":wpilib_interface",
":swerve_publisher",
+ "//y2024_swerve/control_loops:swerve_control_loops",
"//frc971/can_logger",
"//aos/network:message_bridge_client",
"//aos/network:message_bridge_server",
diff --git a/y2024_swerve/constants/BUILD b/y2024_swerve/constants/BUILD
index 42cff81..ae707f3 100644
--- a/y2024_swerve/constants/BUILD
+++ b/y2024_swerve/constants/BUILD
@@ -26,6 +26,7 @@
"common.jinja2",
"common.json",
"//y2024_swerve/constants/calib_files",
+ "//y2024_swerve/control_loops/drivetrain:rotation_json",
"//y2024_swerve/vision/maps",
],
parameters = {},
@@ -41,6 +42,7 @@
"common.jinja2",
"common.json",
"//y2024_swerve/constants/calib_files",
+ "//y2024_swerve/control_loops/drivetrain:rotation_json",
"//y2024_swerve/vision/maps",
],
parameters = {},
diff --git a/y2024_swerve/constants/common.json b/y2024_swerve/constants/common.json
index 393b7b9..300e5f6 100644
--- a/y2024_swerve/constants/common.json
+++ b/y2024_swerve/constants/common.json
@@ -3,5 +3,24 @@
{% set pi = 3.141592653589793 %}
"relative_encoder_scale": {{ 2.0 * pi / 4096 }},
"absolute_encoder_scale": {{ 2.0 * pi }}
+ },
+ "rotation": {
+ "zeroing_voltage": 3.0,
+ "operating_voltage": 12.0,
+ "zeroing_profile_params": {
+ "max_velocity": 0.5,
+ "max_acceleration": 3.0
+ },
+ "default_profile_params":{
+ "max_velocity": 12.0,
+ "max_acceleration": 55.0
+ },
+ "range": {
+ "lower_hard": -inf,
+ "upper_hard": inf,
+ "lower": -inf,
+ "upper": inf
+ },
+ "loop": {% include 'y2024_swerve/control_loops/drivetrain/integral_rotation_plant.json' %}
}
}
diff --git a/y2024_swerve/control_loops/BUILD b/y2024_swerve/control_loops/BUILD
index 67fa349..0417af9 100644
--- a/y2024_swerve/control_loops/BUILD
+++ b/y2024_swerve/control_loops/BUILD
@@ -1,5 +1,21 @@
load("//tools/build_rules:js.bzl", "ts_project")
+cc_binary(
+ name = "swerve_control_loops",
+ srcs = [
+ "swerve_control_loops_main.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/time",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops/swerve:swerve_control_loops",
+ "//y2024_swerve/constants:constants_fbs",
+ ],
+)
+
ts_project(
name = "swerve_plotter",
srcs = ["swerve_plotter.ts"],
diff --git a/y2024_swerve/control_loops/drivetrain/BUILD b/y2024_swerve/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..544cf49
--- /dev/null
+++ b/y2024_swerve/control_loops/drivetrain/BUILD
@@ -0,0 +1,22 @@
+genrule(
+ name = "genrule_rotation",
+ outs = [
+ "rotation_plant.h",
+ "rotation_plant.cc",
+ "rotation_plant.json",
+ "integral_rotation_plant.h",
+ "integral_rotation_plant.cc",
+ "integral_rotation_plant.json",
+ ],
+ cmd = "$(location //y2024_swerve/control_loops/python:rotation) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2024_swerve/control_loops/python:rotation",
+ ],
+)
+
+filegroup(
+ name = "rotation_json",
+ srcs = ["integral_rotation_plant.json"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024_swerve/control_loops/python/BUILD b/y2024_swerve/control_loops/python/BUILD
new file mode 100644
index 0000000..0a6e2c6
--- /dev/null
+++ b/y2024_swerve/control_loops/python/BUILD
@@ -0,0 +1,14 @@
+py_binary(
+ name = "rotation",
+ srcs = [
+ "rotation.py",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ visibility = ["//y2024_swerve:__subpackages__"],
+ deps = [
+ "//frc971/control_loops/python:angular_system_current",
+ "//frc971/control_loops/python:controls",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
diff --git a/y2024_swerve/control_loops/python/rotation.py b/y2024_swerve/control_loops/python/rotation.py
new file mode 100644
index 0000000..a75ed5b
--- /dev/null
+++ b/y2024_swerve/control_loops/python/rotation.py
@@ -0,0 +1,57 @@
+#!/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_current
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+import matplotlib
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kRotation = angular_system_current.AngularSystemCurrentParams(
+ name='Rotation',
+ motor=control_loop.KrakenFOC(),
+ G=9.0 / 24.0 * 14.0 / 72.0,
+ J=3.1 / 1000.0,
+ q_pos=0.05,
+ 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=25 * 0.0254,
+ wrap_point=2.0 * numpy.pi)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system_current.PlotKick(kRotation, R)
+ angular_system_current.PlotMotion(kRotation, 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 wrist and integral wrist.'
+ )
+ else:
+ namespaces = ['y2024_swerve', 'control_loops', 'drivetrain']
+ angular_system_current.WriteAngularSystemCurrent(
+ kRotation, argv[1:4], argv[4:7], namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2024_swerve/control_loops/swerve_control_loops_main.cc b/y2024_swerve/control_loops/swerve_control_loops_main.cc
new file mode 100644
index 0000000..813744a
--- /dev/null
+++ b/y2024_swerve/control_loops/swerve_control_loops_main.cc
@@ -0,0 +1,30 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/swerve/swerve_control_loops.h"
+#include "y2024_swerve/constants/constants_generated.h"
+
+using frc971::control_loops::swerve::SwerveControlLoops;
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ frc971::constants::WaitForConstants<y2024_swerve::Constants>(
+ &config.message());
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+
+ frc971::constants::ConstantsFetcher<y2024_swerve::Constants> constants(
+ &event_loop);
+
+ SwerveControlLoops swerve_control_loops(
+ &event_loop, constants.constants().common()->rotation(),
+ constants.constants().robot()->swerve_zeroing(), "/drivetrain");
+
+ event_loop.Run();
+
+ return 0;
+}