package org.usfirst.frc.team5945.robot.subsystems;

import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
 *
 */
public class EncoderTest extends Subsystem {

    // Put methods for controlling this subsystem
    // here. Call these from Commands.
	private static Encoder enc;
	private static WPI_TalonSRX motor = new WPI_TalonSRX(3);
	public static double motorRadius = 0.2, distancePerPulse;

	public EncoderTest() {
		super();

		enc = new Encoder(0,1,false,Encoder.EncodingType.k4X);
		enc.reset();
		distancePerPulse = motorRadius *(2.0*Math.PI)/7;//how much it moves in inches per pulse
		enc.setDistancePerPulse(distancePerPulse);
		System.out.println("Distance???" + distancePerPulse);
		System.out.println("Distance per pulse??" + enc.getDistancePerPulse());
		enc.setReverseDirection(false);
	}

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }

    public int getEncCount() {
    	return enc.get();
    }

    public void resetEnc() {
    	enc.reset();
    }

    public double getEncDistance() {
    	return enc.getDistance();
    }


    public void setMotor(double volt) {
    	motor.set(volt);
    }
}



package org.usfirst.frc.team5945.robot.commands;

import org.usfirst.frc.team5945.robot.Robot;

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

public class RunEncoderMotor extends Command{
	public RunEncoderMotor() {
		requires(Robot.encoderTest);
	}

	protected void initialize() {
		Robot.encoderTest.setMotor(0.5);
	}

	protected void execute() {
		//System.out.println("count" + Robot.encoderTest.getEncCount());
		//System.out.println("Distance traveled: " + Robot.encoderTest.getEncDistance());
	}

	protected boolean isFinished() {
		return false; //condition for distance
	}

	protected void end() {
		System.out.println("End?");
	}

	protected void interrupted() {
		end();
	}
}