Implement the C++ drivetrain trajectory optimizer
This implements the curvature, forwards, and backwards passes, and adds
a test which makes sure the feed forwards gets us close enough to the
end. Also adds a plotting tool (trajectory_plot) to simulate everything
and tune.
Change-Id: I9f8d6088893cc0b7263b3ff0d79667c027604700
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index 585d6b8..d045f45 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -491,11 +491,8 @@
accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
(-12.0 - C[1, 0]) / K5[1, 0]]
- maxa = 0.0
+ maxa = -float('inf')
for a in accelerations:
- if a < 0.0:
- continue
-
U = K5 * a + K3 * v * v + K4 * v
if not (numpy.abs(U) > 12.0 + 1e-6).any():
maxa = max(maxa, a)
@@ -537,11 +534,8 @@
accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
(-12.0 - C[1, 0]) / K5[1, 0]]
- mina = 0.0
+ mina = float('inf')
for a in accelerations:
- if a > 0.0:
- continue
-
U = K5 * a + K3 * v * v + K4 * v
if not (numpy.abs(U) > 12.0 + 1e-6).any():
mina = min(mina, a)