Fixed wheel nonlinearity being inverted.
Change-Id: I4d81658677a205cafda69c12c4209fc936277095
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 7287df1..f088bff 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -114,7 +114,7 @@
void PolyDrivetrain::SetGoal(double wheel, double throttle, bool quickturn,
bool highgear) {
- const double kWheelNonLinearity = 0.3;
+ const double kWheelNonLinearity = 0.4;
// Apply a sin function that's scaled to make it feel better.
const double angular_range = M_PI_2 * kWheelNonLinearity;
@@ -130,6 +130,7 @@
wheel_ = sin(angular_range * wheel) / sin(angular_range);
wheel_ = sin(angular_range * wheel_) / sin(angular_range);
+ wheel_ = 2.0 * wheel - wheel_;
quickturn_ = quickturn;
static const double kThrottleDeadband = 0.05;