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>
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 16773fa..9c5484c 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -24,7 +24,8 @@
radius=None,
dt=0.00505,
enable_voltage_error=True,
- delayed_u=0):
+ delayed_u=0,
+ wrap_point=0.0):
"""Constructs an AngularSystemParams object.
Args:
@@ -46,6 +47,7 @@
self.dt = dt
self.enable_voltage_error = enable_voltage_error
self.delayed_u = delayed_u
+ self.wrap_point = wrap_point
class AngularSystem(control_loop.ControlLoop):
@@ -144,6 +146,8 @@
self.InitializeState()
+ self.wrap_point = numpy.matrix([[self.params.wrap_point]])
+
class IntegralAngularSystem(AngularSystem):
@@ -195,6 +199,8 @@
self.InitializeState()
+ self.wrap_point = numpy.matrix([[self.params.wrap_point]])
+
def RunTest(plant,
end_goal,