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/polydrivetrain.py

diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index a529378..02a61f1 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -170,6 +170,7 @@
 
         self.U_max = self._drivetrain.U_max
         self.U_min = self._drivetrain.U_min
+        self.wrap_point = numpy.matrix(numpy.zeros((2, 1)))
 
     @property
     def robot_radius_l(self):