Tune the spline controller for auto modes
Use the "regular" kalman filter's velocity estimates, since those will
be more stable in the presence of localizer updates.
Change-Id: I9decb651cbdc4a6cf0642ea576c6840e90988803
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 0d100ed..f448760 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -67,9 +67,10 @@
// Returns the current localizer state.
Eigen::Matrix<double, 5, 1> trajectory_state() {
+ // Use the regular kalman filter's left/right velocity because they are
+ // generally smoother.
return (Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
- localizer_->theta(), localizer_->left_velocity(),
- localizer_->right_velocity())
+ localizer_->theta(), DrivetrainXHat()(1), DrivetrainXHat()(3))
.finished();
}