DriveToDistance.java

package frc.commands;

import edu.wpi.first.wpilibj.command.Command;
import frc.robot.Robot;

public class DriveToDistance extends Command {

    @Override
    protected void initialize() {
        Robot.driveTrain.zeroEncoders();
        Robot.driveTrain.rightMotorThree.setInverted(true);
    }

    @Override
    protected void execute() {
        System.out.println("DriveToDistance ran!");
        Robot.driveTrain.driveToDistance(4096 * 10);
    }

    @Override
    protected boolean isFinished() {
        return false;
    }

}

DriveTrain.java

package frc.subsystems;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import frc.commands.TankDrive;
import frc.robot.Robot;

/**
 * This class manages the drive train
 */
public class DriveTrain extends Subsystem {

    // Create motors
    public WPI_TalonSRX leftMotorOne, leftMotorTwo, leftMotorThree, rightMotorOne, rightMotorTwo, rightMotorThree;

    public DriveTrain(int leftMotorOneId, int leftMotorTwoId, int leftMotorThreeId, int rightMotorOneId,
            int rightMotorTwoId, int rightMotorThreeId) {
        // Initialize motors
        leftMotorOne = new WPI_TalonSRX(leftMotorOneId);
        leftMotorTwo = new WPI_TalonSRX(leftMotorTwoId);
        leftMotorThree = new WPI_TalonSRX(leftMotorThreeId);
        rightMotorOne = new WPI_TalonSRX(rightMotorOneId);
        rightMotorTwo = new WPI_TalonSRX(rightMotorTwoId);
        rightMotorThree = new WPI_TalonSRX(rightMotorThreeId);

        // Set motors to follow masters
        leftMotorOne.follow(leftMotorThree);
        leftMotorTwo.follow(leftMotorThree);
        rightMotorOne.follow(rightMotorThree);
        rightMotorTwo.follow(rightMotorThree);

        leftMotorOne.setInverted(InvertType.FollowMaster);
        leftMotorTwo.setInverted(InvertType.FollowMaster);
        rightMotorOne.setInverted(InvertType.FollowMaster);
        rightMotorTwo.setInverted(InvertType.FollowMaster);

        rightMotorThree.configFactoryDefault();
        rightMotorThree.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
        rightMotorThree.configMotionCruiseVelocity(10000);
        rightMotorThree.configMotionAcceleration(4663);
        rightMotorThree.config_kP(0, .125);
        rightMotorThree.config_kI(0, 0.0);
        rightMotorThree.config_kD(0, 0.0);
        rightMotorThree.config_kF(0, 0.5);
        rightMotorThree.setSensorPhase(true);
        rightMotorThree.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1);

        leftMotorThree.configFactoryDefault();
        leftMotorThree.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
        leftMotorThree.configMotionCruiseVelocity(10000);
        leftMotorThree.configMotionAcceleration(4663);
        leftMotorThree.config_kP(0, .125);
        leftMotorThree.config_kI(0, 0.0);
        leftMotorThree.config_kD(0, 0.0);
        leftMotorThree.config_kF(0, 0.5);
        leftMotorThree.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1);
    }

    public void driveToDistance(int counts) {
        rightMotorThree.set(ControlMode.MotionMagic, counts);
        leftMotorThree.set(ControlMode.MotionMagic, counts);
    }

    /**
     * Brakes all motors on the drive train
     */
    public void brake() {
        leftMotorThree.stopMotor();
        rightMotorThree.stopMotor();
    }

    public void zeroEncoders() {
        leftMotorThree.setSelectedSensorPosition(0, 0, 30);
        rightMotorThree.setSelectedSensorPosition(0, 0, 30);
    }

    @Override
    protected void initDefaultCommand() {
        setDefaultCommand(new TankDrive());
    };

}