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/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index 3c1824c..7dea1dc 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -43,6 +43,10 @@
return spline_.DTheta(alpha) / spline_.DPoint(alpha).norm();
}
+ double DThetaDt(double distance, double velocity) const {
+ return DTheta(distance) * velocity;
+ }
+
// Returns the angular acceleration as a function of distance.
double DDTheta(double distance) const;