Do some initial y2020 drivetrain tuning
1) Makes the drivetrain replay work in a multinode world.
2) Tweaks localizer and spline constants.
3) Initializes the localizer at the start of auto.
4) Runs a simple s-spline in auto, just for kicks.
Change-Id: I1cf990ee8d0c0811fa1e8de7b62209eb0260fa29
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index b28334c..051e71a 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -138,7 +138,7 @@
(::Eigen::DiagonalMatrix<double, 5>().diagonal()
<< 1.0 / ::std::pow(0.12, 2),
1.0 / ::std::pow(0.12, 2), 1.0 / ::std::pow(0.1, 2),
- 1.0 / ::std::pow(1.5, 2), 1.0 / ::std::pow(1.5, 2))
+ 1.0 / ::std::pow(1.0, 2), 1.0 / ::std::pow(1.0, 2))
.finished()
.asDiagonal();
const ::Eigen::DiagonalMatrix<double, 2> R =