Chief Delphi - View Single Post - CANTalon w/Encoder DriveStraight Command example

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