Robot Drive Train Shakes After Canceling A DriveFor Command

Technical Java
1

I have created a command, DriveFor, which takes feet and speed and moves using our encoder that is linked to the PIDSubsystem of the DriveTrain. When cancelling that command or disabling while running that command results in our robot shaking itself forward slowly. I have linked to two videos of the shaking and posted the code below.

DriveFor Command:


package org.usfirst.frc.team5546.robot.commands.driveTrain;

import org.usfirst.frc.team5546.robot.Robot;

import edu.wpi.first.wpilibj.command.Command;

/**
 *
 */
public class DriveFor extends Command {

	boolean finished = false;
	double distance = 0;
	double speed;
	
    public DriveFor(double feet, double _speed) {
        // Use requires() here to declare subsystem dependencies
        requires(Robot.driveTrain);
        this.distance = feet * Robot.driveTrain.DISTANCE_PER_FOOT;
        speed = _speed;
        System.out.println(feet);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	Robot.driveTrain.rotate = false;
    	Robot.driveTrain.squaredInputs = false;
    	Robot.driveTrain.encoderLeft.reset();
    	Robot.driveTrain.imu.reset();
    	Robot.driveTrain.getPIDController().setPID(4, 0.1, 0);
    	Robot.driveTrain.setSetpointRelative(distance);
    	Robot.driveTrain.spd = speed;
    	System.out.println("Setpoint: " + Robot.driveTrain.getSetpoint());
    	System.out.println("Position: " + Robot.driveTrain.getPosition());
    	System.out.println("Position - Setpoint: " + (Robot.driveTrain.getPosition() - Robot.driveTrain.getSetpoint()));
    	Robot.driveTrain.setAbsoluteTolerance(100);
    	Robot.driveTrain.enable();
    	//System.out.println(Robot.driveTrain.getSetpoint());
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	//System.out.println("Executing DriveFor.");
    	//Robot.driveTrain.enable();
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	//return finished;
        return Robot.driveTrain.onTarget();// || Robot.oi.cancelGearBtn.get();
    }

    // Called once after isFinished returns true
    protected void end() {
    	System.out.println("Finished DriveFor");
    	Robot.driveTrain.disable();
    }

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

Drive for 12 feet at .5 speed:

new DriveFor(12, 0.5);

Videos (Let me know if access is wrong):

2

We would also need to see your drivetrain class.

3

Here is the DriveTrain class:


package org.usfirst.frc.team5546.robot.subsystems;

import org.usfirst.frc.team5546.robot.ADIS16448_IMU;
import org.usfirst.frc.team5546.robot.Robot;
import org.usfirst.frc.team5546.robot.RobotMap;
import org.usfirst.frc.team5546.robot.commands.driveTrain.Drive;

import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.VictorSP;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;

/**
 *
 */
public class DriveTrain extends PIDSubsystem {
	
	public ADIS16448_IMU imu;
	public Encoder encoderLeft;
	public AnalogInput ultrasonic;
	
	public final double DISTANCE_PER_FOOT = 1287;
	
	public boolean rotate = false;
	boolean inverted;
	public boolean squaredInputs = false;

    VictorSP leftFront, leftBack, rightFront, rightBack;
    
    SpeedControllerGroup left, right;
    
    DifferentialDrive drive;
    
	public double spd;
	public double distanceToPeg = 0;
	public double centeringDirection = 1;
	public double centerOffset = 0;
	public double distanceBack = 0;
	
	public DriveTrain() {
		super(4, 0.1, 0);
    	setAbsoluteTolerance(20);
		leftFront = new VictorSP(RobotMap.LEFT_FRONT_MOTOR);
		leftBack = new VictorSP(RobotMap.LEFT_BACK_MOTOR);
		rightFront = new VictorSP(RobotMap.RIGHT_FRONT_MOTOR);
		rightBack = new VictorSP(RobotMap.RIGHT_BACK_MOTOR);
		
		left = new SpeedControllerGroup(leftFront, leftBack);
		right = new SpeedControllerGroup(rightFront, rightBack);
		
		drive = new DifferentialDrive(left, right);
		
		imu = new ADIS16448_IMU(); 
		encoderLeft = new Encoder(0, 1);
    	ultrasonic = new AnalogInput(0);
    	
    	spd = 0;
	}
	
	public void driveTank(double leftSpeed, double rightSpeed) {
		setInverted();
		double l, r;
		if(inverted) {
			l = -rightSpeed;
			r = -leftSpeed;
		} else {
			l = leftSpeed;
			r = rightSpeed;
		}
		drive.tankDrive(l, r);
	}
	
	public void driveTankSquared(double leftSpeed, double rightSpeed) {
		setInverted();
		double l, r;
		if(inverted) {
			l = -rightSpeed;
			r = -leftSpeed;
		} else {
			l = leftSpeed;
			r = rightSpeed;
		}
		drive.tankDrive(l, r, true);
	}

    public void initDefaultCommand() {
    	setDefaultCommand(new Drive());
    }
    
    public void setInverted() {
    	inverted = Robot.driveTrainInverted;
    }
    
    protected double returnPIDInput() {
        if (rotate) {
        	return imu.getAngleZ();
        } else {
        	return encoderLeft.getDistance();
        }
    }

    protected void usePIDOutput(double output) {
    	if (rotate) {
    		drive.tankDrive(output * spd, -output * spd);
    	} else if(squaredInputs) {
    		drive.tankDrive(output * spd * 2.5, output * spd * 2.5, true);
    	} else {
    		drive.tankDrive(output * spd * 2.5, output * spd * 2.5);
    	}
    }
}
4

As a rule, a violent shaking like that is often two different things sending opposing commands to the motors. In some cases, the proper use of requires() helps with this.

5

This was my first initial thought too. But I think the PID loop is still running on the subsystem when the command is interrupted. Try adding the disable call on the subsystem in the interrupted method too.

6

When canceling a command (and possibly disabling, too, not sure), the end() method is not called; the interrupted() method is. Adding a call to end() to interrupted() should resolve this.

Oh, sniped.

7

This worked! Thanks so much!

8