package org.robockets.stronghold.robot.drivetrain;

import org.robockets.stronghold.robot.Robot;

import edu.wpi.first.wpilibj.command.Command;

/**
 *
 */
public class AssistedTranslate extends Command {
	private AssistedTranslateType pidType;
	private double relativeAngle;

    public AssistedTranslate() {
    	requires(Robot.driveTrain);
    }

    // Called just before this Command runs the first time
    protected void initialize(AssistedTranslateType pidType, double relativeAngle) {
    	this.pidType = pidType;
    	this.relativeAngle = relativeAngle;
    	
    	if (pidType == AssistedTranslateType.COMPASS) {
    		Robot.driveTrain.enableCompassPID();
    		Robot.driveTrain.setAngle(relativeAngle, true);
    	} else if (pidType == AssistedTranslateType.GYRO) {
    		Robot.driveTrain.enableGyroPID();
    		Robot.driveTrain.setAngle(relativeAngle, false);
    	}
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	if (pidType == AssistedTranslateType.COMPASS) {
    		Robot.driveTrain.driveAssisted(0, true);
    	} else if (pidType == AssistedTranslateType.GYRO) {
    		Robot.driveTrain.driveAssisted(0, false);
    	}
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	if (pidType == AssistedTranslateType.COMPASS) {
    		return Robot.driveTrain.compassPID.onTarget();
    	} else if (pidType == AssistedTranslateType.GYRO) {
    		return Robot.driveTrain.gyroPID.onTarget();
    	} else { // WHAT?!
    		return true;
    	}
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.driveTrain.stop();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	end();
    }
}

@@ -303,141 +303,8 @@
 ate(
-) %7B%0A    %09requires(Robot.driveTrain);%0A    %7D%0A%0A    // Called just before this Command runs the first time%0A    protected void initialize(
 Assi
@@ -350,24 +350,57 @@
 iveAngle) %7B%0A
+    %09requires(Robot.driveTrain);%0A
     %09this.pi
@@ -771,32 +771,132 @@
 ;%0A    %09%7D%0A    %7D%0A%0A
+    // Called just before this Command runs the first time%0A    protected void initialize() %7B%0A    %7D%0A%0A
     // Called re

Move initializer stuff to init instead of the initialize method