X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fdriving%2FDriveDistance.java;fp=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fdriving%2FDriveDistance.java;h=582cd80b69abb799a89a7b3afdcd56432d22a738;hp=bdd5a69adb91f9df52936622a45ca2aacaf72c27;hb=c726d04ade36dead56cd4b2f1b91a341ce90bdb6;hpb=7cd65a82e59374ba6cae30c6f67e358717f7ee48 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 bdd5a69..582cd80 100755 --- 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,6 +46,7 @@ public class DriveDistance extends Command { protected void initialize() { this.driveTrain.resetEncoders(); this.driveController.setSetPoint(this.target); + this.zeroAngle = driveTrain.getAngle(); } @Override