X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fdriving%2FTurnForAngle.java;h=b95c8223a69fccad10d0f3d3cc2bc882129bb6ed;hb=b70398a7d5ac5f4ce64156d84227ccb4828c0615;hp=f9f5c7f0e285676a21b0548baa6d3a80df75879c;hpb=571fb5c99d2323fed344b332f687597cd53d4d31;p=3501%2F2017steamworks

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
index f9f5c7f..b95c822 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
@@ -35,11 +35,16 @@ public class TurnForAngle extends Command {
     this.direction = direction;
     this.maxTimeOut = maxTimeOut;
     this.target = Math.abs(angle);
-    this.zeroAngle = driveTrain.getAngle();
 
-    this.gyroP = driveTrain.defaultGyroP;
-    this.gyroI = driveTrain.defaultGyroI;
-    this.gyroD = driveTrain.defaultGyroD;
+    if (angle > 90) {
+      this.gyroP = driveTrain.largeTurnP;
+      this.gyroI = driveTrain.largeTurnI;
+      this.gyroD = driveTrain.largeTurnD;
+    } else {
+      this.gyroP = driveTrain.smallTurnP;
+      this.gyroI = driveTrain.smallTurnI;
+      this.gyroD = driveTrain.smallTurnD;
+    }
 
     this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD);
     this.gyroController.setDoneRange(1);
@@ -50,6 +55,7 @@ public class TurnForAngle extends Command {
   protected void initialize() {
     this.driveTrain.resetEncoders();
     this.gyroController.setSetPoint(this.target);
+    this.zeroAngle = driveTrain.getAngle();
   }
 
   @Override