From 5ec1f70f431c5dbd0f0a0d140d9ae691d2c66530 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <hfocus77@gmail.com>
Date: Thu, 7 Mar 2019 18:45:58 -0700
Subject: [PATCH] Configuire DriveAbsoluteTurnPID To Nearly Match
 DriveRelativeTurnPID

---
 .../robot/commands/DriveAbsoluteTurnPID.java   | 18 ++++++++++++++++++
 .../frc4388/robot/subsystems/Drive.java        |  3 ---
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnPID.java
index de1dbb3..5498c45 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnPID.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnPID.java
@@ -7,36 +7,54 @@
 
 package org.usfirst.frc4388.robot.commands;
 
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
 import edu.wpi.first.wpilibj.command.Command;
 
 public class DriveAbsoluteTurnPID extends Command {
 
+  private double absoluteTurnAngleDeg;
+	private MPSoftwareTurnType turnType;
+
+  /**
+   * @param absoluteTurnAngleDeg The angle on the NavX for the drive train to turn to
+   * @param turnType The type of turn to get to the angle
+   */
   public DriveAbsoluteTurnPID(double absoluteTurnAngleDeg, MPSoftwareTurnType turnType) {
+    requires(Robot.drive);
+    this.absoluteTurnAngleDeg = absoluteTurnAngleDeg;
+		this.turnType = turnType;
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    Robot.drive.setRelativeTurnPID(absoluteTurnAngleDeg, 0.3, 0.1, turnType);
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
+  protected void execute() {
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
+    return Robot.drive.isFinished();
   }
 
   // Called once after isFinished returns true
   @Override
   protected void end() {
+    Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
   }
 
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   @Override
   protected void interrupted() {
+    end();
   }
 }
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
index da26c21..5fdc9b4 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
+++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
@@ -910,8 +910,5 @@ public class Drive extends Subsystem implements ControlLoopable
 			}
 			SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0);
 		}
-	}
-	public void turnToAngle(double targetAngle, double maxError, double maxPrevError, MPSoftwareTurnType turnType) {
-		setRelativeTurnPID(targetAngle - getGyroAngleDeg(), maxError, maxPrevError, turnType);
 	}	
 }
\ No newline at end of file