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

import edu.wpi.first.wpilibj.MotorSafety;
import edu.wpi.first.wpilibj.MotorSafetyHelper;
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 m_safetyHelper;
    public static final double kDefaultExpirationTime = MotorSafety.DEFAULT_SAFETY_EXPIRATION;

    protected SpeedController m_driveController;
    
    SwerveWheelDrive() {
    	setupMotorSafety();
    }

    private void setupMotorSafety() {
        m_safetyHelper = new MotorSafetyHelper(this);
        this.setExpiration(kDefaultExpirationTime);
        this.setSafetyEnabled(true);
    }

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

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

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

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

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

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

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

}

@@ -139,16 +139,60 @@
 Helper;%0A
+import edu.wpi.first.wpilibj.PIDController;%0A
 import e
@@ -407,16 +407,20 @@
 l angle.
+%0A * 
   %0A * @a
@@ -529,18 +529,16 @@
 yHelper 
-m_
 safetyHe
@@ -638,16 +638,51 @@
 RATION;%0A
+    %0A    protected int wheelNumber;
 %0A    pro
@@ -704,18 +704,16 @@
 troller 
-m_
 driveCon
@@ -729,65 +729,895 @@
     
-%0A    SwerveWheelDrive() %7B%0A    %09setupMotorSafety();%0A    %7D%0A
+protected PIDController steeringController;%0A    %0A    SwerveWheelDrive(%0A    %09%09int wheelNumber, %0A    %09%09SpeedController driveController, %0A    %09%09PIDController steeringController) %7B%0A        this.wheelNumber = wheelNumber;%0A        this.driveController = driveController;%0A        this.steeringController = steeringController;%0A    %09setupMotorSafety();%0A    %7D%0A%0A    public void setWheelSpeed(double speed) %7B%0A        driveController.set(speed);%0A    %7D%0A    %0A    public void setSteeringAngle(double angle) %7B%0A        steeringController.setSetpoint(angle);%0A    %7D%0A    %0A    public void resetSteering() %7B%0A    %09boolean isEnabled = steeringController.isEnable();%0A    %09steeringController.reset();%0A    %09steeringEnable(isEnabled);%0A    %7D%0A    %0A    public void steeringEnable(boolean enable) %7B%0A    %09if (enable) %7B%0A    %09%09steeringController.enable();%0A    %09%7D else %7B%0A    %09%09steeringController.disable();%0A    %09%7D%0A    %7D%0A    
 %0A   
@@ -1651,34 +1651,32 @@
 ety() %7B%0A        
-m_
 safetyHelper = n
@@ -1855,34 +1855,32 @@
 eout) %7B%0A        
-m_
 safetyHelper.set
@@ -1955,34 +1955,32 @@
 %0A        return 
-m_
 safetyHelper.get
@@ -2043,34 +2043,32 @@
 %0A        return 
-m_
 safetyHelper.isA
@@ -2119,18 +2119,16 @@
 r() %7B%0A%09%09
-m_
 driveCon
@@ -2157,18 +2157,16 @@
     if (
-m_
 safetyHe
@@ -2179,18 +2179,16 @@
 = null) 
-m_
 safetyHe
@@ -2272,18 +2272,16 @@
         
-m_
 safetyHe
@@ -2382,10 +2382,8 @@
 urn 
-m_
 safe

Implement swerve wheel drive.