competition fixes

[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 8af1bcbfc3f9e21d9756dc808916539473aedcdf..7b721e0a46dd1a6bba47e1cdf9dda7e9447e6e08 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -1,47 +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
+ * 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, double motorVal) {
-    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();
   }
 }