commit e8a54c0c5505b4d0d11102b309016fb78e168da3
author Austin Schuh <austin.linux@gmail.com> Mon Mar 05 00:25:58 2018 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 10 19:44:32 2018 -0800
tree 473e16adb91ab676f27e357fb421cfc9855c9f6c
parent ab15c4d029b14c076d2f01a7358a00fcf987d762

Drivetrain now kind of works.

Change-Id: Ib8a518f849553949fff1095421cef1fd6f977f44

frc971/control_loops/drivetrain/drivetrain_config.h

diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index c0c7482..6de9883 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -67,6 +67,8 @@
 
   double quickturn_wheel_multiplier;
 
+  double wheel_multiplier;
+
   // Converts the robot state to a linear distance position, velocity.
   static Eigen::Matrix<double, 2, 1> LeftRightToLinear(
       const Eigen::Matrix<double, 7, 1> &left_right) {

frc971/control_loops/drivetrain/drivetrain_lib_test.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 594d504..5faae63 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -57,6 +57,7 @@
       0,
 
       0.25,
+      1.00,
       1.00};
 
   return kDrivetrainConfig;

frc971/control_loops/drivetrain/polydrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index a842f50..97df2cf 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -121,8 +121,10 @@
   wheel_ = 2.0 * wheel - wheel_;
   quickturn_ = quickturn;
 
-  if (!quickturn_) {
+  if (quickturn_) {
     wheel_ *= dt_config_.quickturn_wheel_multiplier;
+  } else {
+    wheel_ *= dt_config_.wheel_multiplier;
   }
 
   static const double kThrottleDeadband = 0.05;