Follow multiple 6 splines
Convert from a 4 to a 6 spline by default, and use a vector of them.
This requires continuity in the angular velocity of the spline that is
not yet checked.
Change-Id: I31487d03916f4831095d922745ba78507895a683
diff --git a/frc971/control_loops/drivetrain/spline.h b/frc971/control_loops/drivetrain/spline.h
index c5b94cc..f60eea5 100644
--- a/frc971/control_loops/drivetrain/spline.h
+++ b/frc971/control_loops/drivetrain/spline.h
@@ -165,12 +165,23 @@
const ::Eigen::Matrix<double, 2, N - 3> dddspline_polynomial_;
};
-typedef NSpline<4> Spline;
+typedef NSpline<6> Spline;
// Converts a 4 control point spline into
::Eigen::Matrix<double, 2, 6> Spline4To6(
const ::Eigen::Matrix<double, 2, 4> &control_points);
+template <int N>
+::Eigen::Matrix<double, 2, N> TranslateSpline(
+ const ::Eigen::Matrix<double, 2, N> &control_points,
+ const ::Eigen::Matrix<double, 2, 1> translation) {
+ ::Eigen::Matrix<double, 2, N> ans = control_points;
+ for (size_t i = 0; i < N; ++i) {
+ ans.template block<2, 1>(0, i) += translation;
+ }
+ return ans;
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971