Configuire DriveAbsoluteTurnPID To Nearly Match DriveRelativeTurnPID

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();
   }
 }

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);
 	}	
 }