fix stuff

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

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 f9f5c7f0e285676a21b0548baa6d3a80df75879c..44434c694531aa65751e95f307fe740ffbf1378a 100755 (executable)
--- 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,10 @@ 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;
+    this.gyroP = driveTrain.turnP;
+    this.gyroI = driveTrain.turnI;
+    this.gyroD = driveTrain.turnD;
 
     this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD);
     this.gyroController.setDoneRange(1);
@@ -50,6 +49,7 @@ public class TurnForAngle extends Command {
   protected void initialize() {
     this.driveTrain.resetEncoders();
     this.gyroController.setSetPoint(this.target);
+    this.zeroAngle = driveTrain.getAngle();
   }
 
   @Override
@@ -70,6 +70,7 @@ public class TurnForAngle extends Command {
     }
 
     this.driveTrain.setMotorValues(leftDrive, rightDrive);
+    System.out.println(this.driveTrain.getAngle() - this.zeroAngle);
   }
 
   @Override