X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fdriving%2FDriveDistance.java;h=47fc46ea0303ed6689b3c2a03b6c918b30407c8f;hp=8af1bcbfc3f9e21d9756dc808916539473aedcdf;hb=e12d6901671044dfcf7eb0380c8193ec69bbc63d;hpb=0788fd3df6f9297dd1d22d41acb4f6c23ac9e8c4

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 8af1bcb..47fc46e 100755
--- 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,98 @@
 package org.usfirst.frc.team3501.robot.commands.driving;
 
+import org.usfirst.frc.team3501.robot.MathLib;
 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 PIDController driveController;
+  private PIDController gyroController;
+  private Preferences prefs;
+
+  private double target;
+  private double gyroP;
+  private double gyroI;
+  private double gyroD;
+
+  private double driveP;
+  private double driveI;
+  private double driveD;
 
   public DriveDistance(double distance, double motorVal) {
-    requires(Robot.getDriveTrain());
+    requires(driveTrain);
+    this.driveP = driveTrain.driveP;
+    this.driveI = driveTrain.driveI;
+    this.driveD = driveTrain.driveD;
+    this.gyroP = driveTrain.defaultGyroP;
+    this.gyroI = driveTrain.defaultGyroI;
+    this.gyroD = driveTrain.defaultGyroD;
+    this.driveController = new PIDController(this.driveP, this.driveI,
+        this.driveD);
+    this.driveController.setDoneRange(0.5);
+    this.driveController.setMaxOutput(1.0);
+    this.driveController.setMinDoneCycles(5);
+
+    this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD);
+    this.gyroController.setDoneRange(1);
+    this.gyroController.setMinDoneCycles(5);
   }
 
-  // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    this.driveTrain.resetEncoders();
+    this.driveTrain.resetGyro();
+    this.driveController.setSetPoint(this.target);
+    this.gyroController.setSetPoint(this.driveTrain.getZeroAngle());
   }
 
-  // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    double xVal = 0;
+    double yVal = this.driveController
+        .calcPID(this.driveTrain.getAvgEncoderDistance());
+
+    if (this.driveTrain.getAngle() - this.driveTrain.getZeroAngle() < 30) {
+      xVal = -this.gyroController
+          .calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle());
+    }
+    // System.out.println("turn: " + xVal);
+    double leftDrive = MathLib.calcLeftTankDrive(-xVal, yVal);
+    double rightDrive = MathLib.calcRightTankDrive(xVal, -yVal);
+
+    this.driveTrain.setMotorValues(leftDrive, rightDrive);
+
+    System.out.println(driveTrain.getAvgEncoderDistance());
+    // System.out.println("motorval: " + yVal);
   }
 
-  // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+    boolean isDone = this.driveController.isDone();
+    if (timeSinceInitialized() >= maxTimeOut || isDone)
+      System.out.println("time: " + timeSinceInitialized());
+    return timeSinceInitialized() >= maxTimeOut || isDone;
   }
 
-  // Called once after isFinished returns true
   @Override
   protected void end() {
+    driveTrain.stop();
   }
 
-  // Called when another command which requires one or more of the same
-  // subsystems is scheduled to run
   @Override
   protected void interrupted() {
+    end();
   }
 }