Add a time varying RK4 integration
Splines need a RK4 integration which is a function of time.
Change-Id: Iddd3dffe50beaf3b9d5b70e58ee88df5d727b853
diff --git a/frc971/control_loops/runge_kutta.h b/frc971/control_loops/runge_kutta.h
index c3d47ed..61d674e 100644
--- a/frc971/control_loops/runge_kutta.h
+++ b/frc971/control_loops/runge_kutta.h
@@ -19,6 +19,20 @@
return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
+// Implements Runge Kutta integration (4th order). This integrates dy/dt = fn(t,
+// y). It must have the call signature of fn(double t, T y). The
+// integration starts at an initial value y, and integrates for dt.
+template <typename F, typename T>
+T RungeKutta(const F &fn, T y, double t, double dt) {
+ const double half_dt = dt * 0.5;
+ T k1 = dt * fn(t, y);
+ T k2 = dt * fn(t + half_dt, y + k1 / 2.0);
+ T k3 = dt * fn(t + half_dt, y + k2 / 2.0);
+ T k4 = dt * fn(t + dt, y + k3);
+
+ return y + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
+}
+
// Implements Runge Kutta integration (4th order). fn is the function to
// integrate. It must take 1 argument of type T. The integration starts at an
// initial value X, and integrates for dt.