X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fdriving%2FDriveDistance.java;h=b82e7d62df1aae12e22fe8ebb0a7478430f2afa4;hb=35527abe487a3d3cee1ddb10b5c4debc2a2780e4;hp=2af7387150068f67bf2e102dd5044a814f2fc73e;hpb=c0e9f91162b9c4b88e1ae5610730e91db86a6dd0;p=3501%2F2017steamworks

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 2af7387..b82e7d6 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -1,46 +1,76 @@
 package org.usfirst.frc.team3501.robot.commands.driving;
 
 import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.utils.PIDController;
 
+import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.command.Command;
 
 /**
  * This command makes the robot drive a specified distance using encoders on the
  * robot and using a feedback loop
  *
- * parameters:
- * distance the robot will move in inches
+ * parameters: distance the robot will move in inches motorVal: the motor input
+ * to set the motors to
  */
 public class DriveDistance extends Command {
+  private DriveTrain driveTrain = Robot.getDriveTrain();
+  private double maxTimeOut;
+  private double target;
+  private double zeroAngle;
+  private Preferences prefs;
+  private PIDController driveController;
 
-  public DriveDistance(double distance) {
-    requires(Robot.getDriveTrain());
+  private double driveP;
+  private double driveI;
+  private double driveD;
+  private double gyroP;
+
+  public DriveDistance(double distance, double motorVal) {
+    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.setMaxOutput(1.0);
+    this.driveController.setMinDoneCycles(5);
   }
 
-  // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    this.driveTrain.resetEncoders();
+    this.driveController.setSetPoint(this.target);
   }
 
-  // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    double xVal = gyroP * (driveTrain.getAngle() - zeroAngle);
+    double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance());
+
+    double leftDrive = yVal - xVal;
+    double rightDrive = yVal + xVal;
+    this.driveTrain.setMotorValues(leftDrive, rightDrive);
   }
 
-  // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+    return timeSinceInitialized() >= maxTimeOut
+        || this.driveController.isDone();
   }
 
-  // Called once after isFinished returns true
   @Override
   protected void end() {
   }
 
-  // Called when another command which requires one or more of the same
-  // subsystems is scheduled to run
   @Override
   protected void interrupted() {
+    end();
   }
 }