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 bd923a2af73427ffa69ad842431c1736feb4a12e..cb8227f16017bdde5ef8c0c04c54ed645faad3a6 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -6,6 +6,7 @@ import org.usfirst.frc.team3501.robot.utils.PIDController;
 
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * This command makes the robot drive a specified distance using encoders on the
@@ -22,11 +23,13 @@ public class DriveDistance extends Command {
   private Preferences prefs;
   private PIDController driveController;
 
-  private double gyroP;
+  private double driveStraightGyroP;
 
   public DriveDistance(double distance, double maxTimeOut) {
     requires(driveTrain);
     this.target = distance;
+    driveStraightGyroP = SmartDashboard.getNumber(driveTrain.DRIVE_GYRO_P_Val,
+        driveTrain.PID_ERROR);
 
     this.driveController = driveTrain.getDriveController();
     this.driveController.setDoneRange(0.5);
@@ -42,12 +45,18 @@ public class DriveDistance extends Command {
 
   @Override
   protected void execute() {
-    double xVal = gyroP * (driveTrain.getAngle() - zeroAngle);
+    double xVal = driveStraightGyroP * (driveTrain.getAngle() - zeroAngle);
     double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
 
     double leftDrive = yVal - xVal;
     double rightDrive = yVal + xVal;
     this.driveTrain.setMotorValues(leftDrive, rightDrive);
+
+    System.out.println(
+        "PID VALS: " + driveController.getP() + " " + driveController.getI()
+            + " " + driveController.getD());
+
+    System.out.println(driveTrain.getAvgEncoderDistance());
   }
 
   @Override