CANTalon w/Encoder DriveStraight Command example

cybersader13 · #1 16-01-2017, 12:31

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.

Code:

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

Code:

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

CSkillet · #2 16-01-2017, 20:34

Quote:

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

cybersader13 · #3 17-01-2017, 08:55

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

Code:

talon.configEncoderCodesPerRev(360)

or

Code:

talon.configEncoderCodesPerRev(360 * 4)

We have things working with the former.

Then there is also this

Code:

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

Code:

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!

cybersader13 · #4 22-01-2017, 14:03

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.

Code:

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

cnc4 · #5 29-01-2017, 18:27

I would personally do in isFinished:

Quote:

Math.abs(getPos()) < tolerance

Thread Tools
Show Printable Version Email this Page
Display Modes	Rate This Thread
Linear Mode Switch to Hybrid Mode Switch to Threaded Mode	Rate This Thread: