update code

[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 8a76d0d9e890b062d2578dcd9e7b5908398036a8..24d6235b04650d832c9579dd7649d09452e03ff3 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -42,10 +42,12 @@ public class DriveDistance extends Command {
   protected void initialize() {
     this.driveTrain.resetEncoders();
     this.driveController.setSetPoint(this.target);
+    zeroAngle = driveTrain.getAngle();
   }
 
   @Override
   protected void execute() {
+    System.out.printf("angle: %.2f\t", driveTrain.getAngle() - zeroAngle);
     double xVal = driveStraightGyroP * (driveTrain.getAngle() - zeroAngle);
     double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
 
@@ -53,11 +55,17 @@ public class DriveDistance extends Command {
     double rightDrive = yVal + xVal;
     this.driveTrain.setMotorValues(leftDrive, rightDrive);
 
-    System.out.println(
-        "PID Vals: " + driveController.getP() + " " + driveController.getI()
-            + " " + driveController.getD());
+    // SmartDashboard.putNumber("x", xVal);
+    // SmartDashboard.putNumber("y", yVal);
+    //
+    // SmartDashboard.putNumber("left", driveTrain.getLeftEncoderDistance());
+    // SmartDashboard.putNumber("right", driveTrain.getRightEncoderDistance());
+    //
+    // System.out.printf("x: %.2f\t", xVal);
+    // System.out.printf("y: %.2f\t", yVal);
+    //
+    // System.out.printf("dist: %.2f\n", driveTrain.getAvgEncoderDistance());
 
-    System.out.println("Encoder " + driveTrain.getAvgEncoderDistance());
   }
 
   @Override