commit c73bb22a8045b51ee4db0665641588de15647a8b
author James Kuszmaul <jabukuszmaul@gmail.com> Sun Apr 07 12:15:35 2019 -0700
committer James Kuszmaul <jabukuszmaul@gmail.com> Wed Apr 10 20:18:12 2019 -0700
tree 5c347abbe137728c094b629497b22b714272ac89
parent da049002f96104ead2ef55f3925b35da68f1ee03

Add ability to drive splines backwards

Change-Id: Ib34366ed155625fba56c4f72b9adce0615c6f12d

frc971/control_loops/drivetrain/splinedrivetrain.h

diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index d70cb6d..59175b6 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -71,6 +71,7 @@
   int32_t current_spline_handle_ = 0;  // Current spline told to excecute.
   int32_t current_spline_idx_ = 0;     // Current executing spline.
   bool has_started_execution_ = false;
+  bool drive_spline_backwards_ = false;
 
   ::std::unique_ptr<DistanceSpline> current_distance_spline_;
   ::std::unique_ptr<Trajectory> current_trajectory_;
@@ -105,8 +106,8 @@
   // TODO(alex): pull this out of dt_config.
   const ::Eigen::DiagonalMatrix<double, 5> Q =
       (::Eigen::DiagonalMatrix<double, 5>().diagonal()
-           << 1.0 / ::std::pow(0.07, 2),
-       1.0 / ::std::pow(0.07, 2), 1.0 / ::std::pow(0.2, 2),
+           << 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))
           .finished()
           .asDiagonal();