CANTalon w/Encoder DriveStraight Command example

[cybersader13]

Hi,

Our goal is to create a very simple autonomous command to drive straight some specified distance. Seems like a very common use case, and we have looked through the CTR programmer guide and some code samples (found here https://github.com/CrossTheRoadElec/FRC-Examples AND https://github.com/ozrien/FRC-2016-Public) but none of these sources provide an example that simply does this.

The encoder we are using is this one and we have it wired into the Talon via breakout board:
http://www.andymark.com/E4T-OEM-Mini...-p/am-3132.htm

We have a simple DriveTrain Subsystem -- for sake of this example it is pared down to just a single CANTalon that we are trying to control (source snip to follow). We also have a Command that calls the set() method of the Talon, passing in some desired number of units to move.

First, we are wondering, what actually stops the motor - the encoder should work to stop it internally/automatically once the number of ticks have been counted - right? The Command should not have to do this when done. Expect all isFinished() is responsible for is telling the command scheduler that this command can be cleaned up/removed from executing.

Further, we think the command's execute() method shouldn't have to do anything. Think all we need to do is update the set point of the Talon.

Here is a bit of code showing what we are trying to do. What we see in practice is the position of the encoder updating, but the motor is not being stopped by the encoder control system. Rather we have to stop it in the command and we know this will not produce anything precise. Thanks in advance for any pointers/direction.

public class DriveTrain extends Subsystem {
	public CANTalon talon;
	
	public DriveTrain() {
		talon = new CANTalon(0);
		talon.setControlMode(CANTalon.TalonControlMode.Position.value);
		//This should be setting the set point to move encoder ticks
		talon.set(0);
		talon.setFeedbackDevice(FeedbackDevice.QuadEncoder);
		talon.configEncoderCodesPerRev(1440);

		talon.configNominalOutputVoltage(+0.0f, -0.0f);
		talon.configPeakOutputVoltage(+6.0f, -6.0f);
		talon.setAllowableClosedLoopErr(0);
		talon.setPID(0.5, 0.001, 0.0);
		talon.setPosition(0);
		talon.enableControl();
	}
...

public class DriveStraight extends Command {
    private DriveTrain driveTrain;
    private double numberOfUnits;
    
    public DriveStraight(double numberOfUnits, DriveTrain driveTrain) {
    	this.numberOfUnits = numberOfUnits;
    	this.driveTrain = driveTrain;
    }

    protected void initialize() {
        driveTrain.talon.reset();
        driveTrain.talon.enable();
        driveTrain.talon.setPosition(0);
        driveTrain.talon.set(numberOfUnits);
    }

    protected void execute() {
    	// Should be nothing in here - right?  
        // Once the new setpoint is set in initialize the work is done.
    }

    protected boolean isFinished() {
    	if (numberOfUnits < 0) {
    		return (driveTrain.talon.getPosition() >= -numberOfUnits);
    	}
        return (driveTrain.talon.getPosition() <= -numberOfUnits);
    }

    protected void end() {
        // Shouldn't have to do this to get it to stop
    	driveTrain.talon.disableControl();
    }
...