CANTalon w/Encoder DriveStraight Command example

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:

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();
    }
...

protected boolean isFinished() {
	if (numberOfUnits &lt; 0) {
		return (driveTrain.talon.getPosition() &gt;= -numberOfUnits);
	}
    return (driveTrain.talon.getPosition() &lt;= -numberOfUnits);
}

This is just from my perspective because I don’t know what values you are getting from driveTrain.talon.getPosition(), but it seems your return equations are off. When the numberOfUnits is negative, driveTrain.talon.getPosition() is also going in the negative direction. So when you take the negative of numberOfUnits when it is already a negative number, its turns to a positive number. So when is driveTrain.talon.getPosition() (a negative number) ever greater than or equal to a positive number? Never. Your return would always be false and so the command would never end. The other equation seems to be off to me to, but I think you get my point now.

Thank you. Yep, the logic in the isFinished() looks odd because of the way the Talon was wired. Setting a positive setpoint was generating negative counts, so we compensated for that in the code.

We have got a simple command working and will post a cleaned up example of what we did tonight in case it helps others in the future.

One thing that I don’t entirely have my head wrapped around yet is the units used for some of the configuration.

For example, with a 360 cycles/rev quad encoder, are we supposed to use

talon.configEncoderCodesPerRev(360) 

or

talon.configEncoderCodesPerRev(360 * 4)

We have things working with the former.

Then there is also this

driveTrain.talon.set(numberOfRevs)

which if we’re right is in terms of revolutions (though javadoc says “In Position mode, outputValue is in encoder ticks…”?) and this

talon.setAllowableClosedLoopErr(allowableError)

which is in terms of cycles (1/360 or 1/(360 *4))? Will nail that down when able to test a bit more.

Again, thanks!

As mentioned, here is the test code we used to learn about encoder-based control of a single talon. Now that we got this simplified code to work, we can move on to more realistic usages.

import com.ctre.CANTalon;
import com.ctre.CANTalon.FeedbackDevice;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc.team5976.robot.subsystems.DriveTrain;

/**
 * Drive the given distance straight (negative values go backwards).
 */
public class DriveStraight extends Command {
    private DriveTrain driveTrain;
    private CANTalon talon;
    private double numberOfRevs;
    private int allowableError = 10;
    private int stableCount;
    private final int requiredNumberOfCounts = 10;
    private double previousError;
    
    public DriveStraight(double distanceInInches, DriveTrain driveTrain) {
	this.driveTrain = driveTrain;
    	talon = driveTrain.talon;
    	numberOfRevs = distanceInInches / (Math.PI * 6);
    	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(360);
		
	talon.setProfile(0);
	talon.configNominalOutputVoltage(+0.0f, -0.0f);
	talon.configPeakOutputVoltage(+6.0f, -6.0f);
	talon.setAllowableClosedLoopErr(allowableError);
	talon.setPosition(0);
	talon.enableControl();
    }

    protected void initialize() {
    	// Get everything in a safe starting state.
        driveTrain.reset();
        talon.reset();
        talon.enable();
        talon.setPosition(0);

        System.out.println("Encoder start position: " + talon.getPosition());
        System.out.println("Talon control mode: " + talon.getControlMode());
        System.out.println("Number of Revolutions: " + numberOfRevs);

        talon.set(numberOfRevs);        
	previousError = 100000; // something big to start with
        stableCount = 0;
    }

    protected void execute() {
    }

    protected boolean isFinished() {
    	double currentError = driveTrain.talon.getError();
    	System.out.println("Encoder position: " + talon.getPosition()+ 
    			" |EP " + talon.getEncPosition() + 
    			" |CErr " + currentError + " |PErr " + previousError +  " |Count " + stableCount +
    			" |SetP " + talon.getSetpoint());
    	if (previousError == currentError) {
    		stableCount++;
    		if (stableCount >= requiredNumberOfCounts) return true;
    	}
    	else {
    		stableCount = 0;
    		previousError = currentError;
    	}
    	return false;
    }

    protected void end() {
    	System.out.println("Encoder stop position: " + talon.getPosition()+ " | " + talon.getEncPosition());
    }

    // Called when another command which requires one or more of the same subsystems is scheduled to run
    protected void interrupted() {
        end();
    }
}

Sample output from running this example looks like this below. We noted the current error (CErr) of 1440 corresponds to the number of quad encoder pulses expected for one full revolution. Also note the value returned from the talon’s getPosition() method is updating with every pass but the value returned by getEncPosition() (EP in the output) is updated less frequently. Also, noted is that getPosition’s value in in terms of revolutions and getEncPosition’s is in terms of pulses.

P = 2 I = 0 D = 50 Ramp = 4
Talon control mode: Position
Number of Revolutions: 1.0
Encoder position: 0.0 |EP 0 |CErr 1440.0 |PErr -9.0 |Count 2 |SetP 1.0 |T Time: 20
Encoder position: 0.0 |EP 0 |CErr 1440.0 |PErr 1440.0 |Count 0 |SetP 1.0 |T Time: 20
Encoder position: 0.001388888888888889 |EP 0 |CErr 1438.0 |PErr 1440.0 |Count 1 |SetP 1.0 |T Time: 20
Encoder position: 0.005555555555555556 |EP 0 |CErr 1424.0 |PErr 1438.0 |Count 0 |SetP 1.0 |T Time: 20
Encoder position: 0.022222222222222223 |EP 0 |CErr 1392.0 |PErr 1424.0 |Count 0 |SetP 1.0 |T Time: 22
Encoder position: 0.05 |EP 0 |CErr 1370.0 |PErr 1392.0 |Count 0 |SetP 1.0 |T Time: 18
Encoder position: 0.0875 |EP 126 |CErr 1314.0 |PErr 1370.0 |Count 0 |SetP 1.0 |T Time: 21
Encoder position: 0.13680555555555557 |EP 126 |CErr 1239.0 |PErr 1314.0 |Count 0 |SetP 1.0 |T Time: 19
Encoder position: 0.19444444444444445 |EP 126 |CErr 1151.0 |PErr 1239.0 |Count 0 |SetP 1.0 |T Time: 21
Encoder position: 0.25833333333333336 |EP 126 |CErr 1054.0 |PErr 1151.0 |Count 0 |SetP 1.0 |T Time: 24
Encoder position: 0.32569444444444445 |EP 126 |CErr 1003.0 |PErr 1054.0 |Count 0 |SetP 1.0 |T Time: 15
Encoder position: 0.3972222222222222 |EP 126 |CErr 898.0 |PErr 1003.0 |Count 0 |SetP 1.0 |T Time: 20
Encoder position: 0.46944444444444444 |EP 645 |CErr 789.0 |PErr 898.0 |Count 0 |SetP 1.0 |T Time: 21

I would personally do in isFinished:

Math.abs(getPos()) < tolerance

Want to add a quick note to this. While in our test scenario (with just one talon and one encoder) we were able to have an execute() method that was empty, we saw this leading to problems when we moved to our real drivetrain with 4 talons, 2 encoders. Specifically it behaved like the watchdog was shutting our system down b/c (we think) it was not sensing that we were in control of the talons given we were not commanding them in any way within execute(). We added calls to talon.set(revsToMove) within execute() and that solved the issue.

Many thanks to veteran team 2062 CORE robotics from Waukesha South, WI for helping us work through this and other issues. They have helped us a ton over the last two years and are a shining example of how to demonstrate the values of gracious professionalism and coopertition.

That is explained in section 16.14 and section 19 of the Talon SRX Software reference manual.