commit 9ea3ff970cd4b176a041fa3ac04f71918abcfe7d
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Jun 14 15:02:15 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Aug 10 11:58:22 2024 -0700
tree 26319ad697d119e603f07971ee90411fac499db7
parent 3f1bbf5b68237d6dad7afa4a7ab68978c8e23f58

Create/support "continuous" control loops

This supplies a wrap_point argument to the control loops code that
makes it so that you can have a system that spins infinitely (as the
swerve modules do) and still control them.

TODO: I observed some idiosyncracies in wrapping behavior during
testing; this likely requires additional tests to be written to validate
that we handle wrapping correctly.

Change-Id: Id4b9065de2b3334c0e8097b28a32916c47a54258
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

frc971/control_loops/python/control_loop.py

diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index a6fcd3e..ec44092 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -353,6 +353,7 @@
         self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
         self.last_U = numpy.matrix(
             numpy.zeros((self.B.shape[1], max(1, self.delayed_u))))
+        self.wrap_point = numpy.matrix(numpy.zeros(self.Y.shape))
 
     def PlaceControllerPoles(self, poles):
         """Places the controller poles.
@@ -451,7 +452,8 @@
             "u_max": MatrixToJson(self.U_max),
             "u_limit_coefficient": MatrixToJson(self.U_limit_coefficient),
             "u_limit_constant": MatrixToJson(self.U_limit_constant),
-            "delayed_u": self.delayed_u
+            "delayed_u": self.delayed_u,
+            "wrap_point": MatrixToJson(self.wrap_point)
         }
         if plant_coefficient_type.startswith('StateFeedbackPlant'):
             result["a"] = MatrixToJson(self.A)
@@ -502,11 +504,13 @@
         if plant_coefficient_type.startswith('StateFeedbackPlant'):
             ans.append(self._DumpMatrix('A', self.A, scalar_type))
             ans.append(self._DumpMatrix('B', self.B, scalar_type))
+            ans.append(
+                self._DumpMatrix('wrap_point', self.wrap_point, scalar_type))
             ans.append('  const std::chrono::nanoseconds dt(%d);\n' %
                        (self.dt * 1e9))
             ans.append(
                 '  return %s'
-                '(A, B, C, D, U_max, U_min, U_limit_coefficient, U_limit_constant, dt, %s);\n'
+                '(A, B, C, D, U_max, U_min, U_limit_coefficient, U_limit_constant, dt, %s, wrap_point);\n'
                 % (plant_coefficient_type, delayed_u_string))
         elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
             ans.append(