competition fixes

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commands / driving / DriveDistance.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
index bdd5a69adb91f9df52936622a45ca2aacaf72c27..7b721e0a46dd1a6bba47e1cdf9dda7e9447e6e08 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -31,14 +31,13 @@ public class DriveDistance extends Command {
     requires(driveTrain);
     this.maxTimeOut = maxTimeOut;
     this.target = distance;
-    this.zeroAngle = driveTrain.getAngle();
 
     this.driveP = driveTrain.driveP;
     this.driveI = driveTrain.driveI;
     this.driveD = driveTrain.driveD;
     this.gyroP = driveTrain.driveStraightGyroP;
     this.driveController = new PIDController(driveP, driveI, driveD);
-    this.driveController.setDoneRange(0.5);
+    this.driveController.setDoneRange(1.0);
     this.driveController.setMaxOutput(1.0);
     this.driveController.setMinDoneCycles(5);
   }
@@ -47,12 +46,13 @@ public class DriveDistance extends Command {
   protected void initialize() {
     this.driveTrain.resetEncoders();
     this.driveController.setSetPoint(this.target);
+    this.zeroAngle = driveTrain.getAngle();
   }
 
   @Override
   protected void execute() {
     double xVal = gyroP * (driveTrain.getAngle() - zeroAngle);
-    double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
+    double yVal = driveController.calcPID(driveTrain.getRightEncoderDistance());
 
     double leftDrive = yVal - xVal;
     double rightDrive = yVal + xVal;