package org.usfirst.frc.team2339.Barracuda.subsystems;

import edu.wpi.first.wpilibj.MotorSafety;
import edu.wpi.first.wpilibj.MotorSafetyHelper;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.SpeedController;

/**
 * Class to provide swerve drive control to one wheel.
 * Wheel must have independent drive motor and steering motor.
 * A steering encoder provides feedback on wheel angle.
 *   
 * @author emiller
 *
 */
public class SwerveWheelDrive implements MotorSafety {
	
    protected MotorSafetyHelper safetyHelper;
    public static final double kDefaultExpirationTime = MotorSafety.DEFAULT_SAFETY_EXPIRATION;
    
    protected int wheelNumber;
    protected SpeedController driveController;
    protected PIDController steeringController;
    
    SwerveWheelDrive(
    		int wheelNumber, 
    		SpeedController driveController, 
    		PIDController steeringController) {
        this.wheelNumber = wheelNumber;
        this.driveController = driveController;
        this.steeringController = steeringController;
    	setupMotorSafety();
    }

    public void setWheelSpeed(double speed) {
        driveController.set(speed);
    }
    
    public void setSteeringAngle(double angle) {
        steeringController.setSetpoint(angle);
    }
    
    public void resetSteering() {
    	boolean isEnabled = steeringController.isEnable();
    	steeringController.reset();
    	steeringEnable(isEnabled);
    }
    
    public void steeringEnable(boolean enable) {
    	if (enable) {
    		steeringController.enable();
    	} else {
    		steeringController.disable();
    	}
    }
    
    private void setupMotorSafety() {
        safetyHelper = new MotorSafetyHelper(this);
        this.setExpiration(kDefaultExpirationTime);
        this.setSafetyEnabled(true);
    }

	@Override
	public void setExpiration(double timeout) {
        safetyHelper.setExpiration(timeout);
	}

	@Override
	public double getExpiration() {
        return safetyHelper.getExpiration();
	}

	@Override
	public boolean isAlive() {
        return safetyHelper.isAlive();
	}

	@Override
	public void stopMotor() {
		driveController.set(0.0);
        if (safetyHelper != null) safetyHelper.feed();
	}

	@Override
	public void setSafetyEnabled(boolean enabled) {
        safetyHelper.setSafetyEnabled(enabled);
	}

	@Override
	public boolean isSafetyEnabled() {
        return safetyHelper.isSafetyEnabled();
	}

	@Override
	public String getDescription() {
		return "Swerve wheel drive";
	}

}

@@ -49,16 +49,99 @@
 stems;%0A%0A
+import org.usfirst.frc.team2339.Barracuda.components.SwerveSteeringPidController;%0A%0A
 import e
@@ -174,16 +174,16 @@
 Safety;%0A
-
 import e
@@ -226,52 +226,8 @@
 er;%0A
-import edu.wpi.first.wpilibj.PIDController;%0A
 impo
@@ -586,103 +586,8 @@
 er;%0A
-    public static final double kDefaultExpirationTime = MotorSafety.DEFAULT_SAFETY_EXPIRATION;%0A
     
@@ -679,19 +679,33 @@
 otected 
-PID
+SwerveSteeringPid
 Controll
@@ -828,11 +828,25 @@
   %09%09
-PID
+SwerveSteeringPid
 Cont
@@ -1126,24 +1126,79 @@
 set(speed);%0A
+        if (safetyHelper != null) safetyHelper.feed();%0A
     %7D%0A    %0A 
@@ -1738,21 +1738,16 @@
         
-this.
 setExpir
@@ -1756,30 +1756,45 @@
 ion(
-kDefaultExpirationTime
+MotorSafety.DEFAULT_SAFETY_EXPIRATION
 );%0A 
@@ -1800,21 +1800,16 @@
         
-this.
 setSafet

Update swerve wheel motor safety and PID controller (to SwerveSteeringPidController)