commit a4226f5f464cde2c3f28b2ae5cea04308685d4e4
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Mar 01 21:29:12 2024 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Mar 01 21:34:01 2024 -0800
tree 3a3896a7f14afff3a766c80e8841b902ef4da5dd
parent 19da3230b9d11cd7463e46a502ff7a37a6bda638

Disable voltage error on 2024 intake pivot

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

frc971/control_loops/python/angular_system.py

diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index beade85..16773fa 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -23,6 +23,7 @@
                  kalman_r_position,
                  radius=None,
                  dt=0.00505,
+                 enable_voltage_error=True,
                  delayed_u=0):
         """Constructs an AngularSystemParams object.
 
@@ -43,6 +44,7 @@
         self.kalman_r_position = kalman_r_position
         self.radius = radius
         self.dt = dt
+        self.enable_voltage_error = enable_voltage_error
         self.delayed_u = delayed_u
 
 
@@ -185,7 +187,8 @@
         self.K_unaugmented = self.K
         self.K = numpy.matrix(numpy.zeros((1, 3)))
         self.K[0, 0:2] = self.K_unaugmented
-        self.K[0, 2] = 1
+        if params.enable_voltage_error:
+            self.K[0, 2] = 1
 
         self.Kff = numpy.concatenate(
             (self.Kff, numpy.matrix(numpy.zeros((1, 1)))), axis=1)