Removed redundant turn_width.
Change-Id: I68d70930c3d25afa05d00e381b73cd34bdfe6f33
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 9fdfe1c..19bbef1 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -139,8 +139,7 @@
}
void DrivetrainMotorsSS::SetPosition(double left, double right, double gyro) {
// Decay the offset quickly because this gyro is great.
- const double offset =
- (right - left - gyro * dt_config_.turn_width) / 2.0;
+ const double offset = (right - left) / 2.0 - gyro * dt_config_.robot_radius;
filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
gyro_ = gyro;
SetRawPosition(left, right);