X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fdriving%2FDriveDistance.java;h=7b721e0a46dd1a6bba47e1cdf9dda7e9447e6e08;hb=9ca89e45fa84b2ec93bc6adf60c7dde1e0a7defb;hp=d981d99e5b0eac798ff3949c9108a609f99dd6c3;hpb=d17d868dd3c4b68452392e321e0980938a4ace76;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 d981d99..7b721e0 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -1,42 +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 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() {
-    requires(Robot.getDriveTrain());
+  private double driveP;
+  private double driveI;
+  private double driveD;
+  private double gyroP;
+
+  public DriveDistance(double distance, double maxTimeOut) {
+    requires(driveTrain);
+    this.maxTimeOut = maxTimeOut;
+    this.target = distance;
+
+    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(1.0);
+    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);
+    this.zeroAngle = driveTrain.getAngle();
   }
 
-  // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    double xVal = gyroP * (driveTrain.getAngle() - zeroAngle);
+    double yVal = driveController.calcPID(driveTrain.getRightEncoderDistance());
+
+    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();
   }
 }