Add a 4 point spline to 6 point spline function
This makes a 6th order spline with the same shape as the provided 4th
order spline.
Change-Id: I14cf6ca0560c5a99ece2152c444dc7509dc7a34f
diff --git a/frc971/control_loops/drivetrain/spline.h b/frc971/control_loops/drivetrain/spline.h
index 7a77082..c5b94cc 100644
--- a/frc971/control_loops/drivetrain/spline.h
+++ b/frc971/control_loops/drivetrain/spline.h
@@ -150,6 +150,10 @@
1.0 / magdxy2 * (dx * dddy - dy * dddx);
}
+ const ::Eigen::Matrix<double, 2, N> &control_points() const {
+ return control_points_;
+ }
+
private:
const ::Eigen::Matrix<double, 2, N> control_points_;
@@ -163,6 +167,10 @@
typedef NSpline<4> Spline;
+// Converts a 4 control point spline into
+::Eigen::Matrix<double, 2, 6> Spline4To6(
+ const ::Eigen::Matrix<double, 2, 4> &control_points);
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971