added encoder rate getters and implemented maximum velocity when ending on the pid commands

Author: dfang314

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java

@@ -76,7 +76,9 @@ protected void execute() {
 	 */
 	@Override
 	protected boolean isFinished() {
-		return moveController.onTarget();
+		return moveController.onTarget()
+				&& Math.abs(dt.getLeftEncRate()) <= Robot.getConst("Maximum Velocity When Stop", 1)
+				&& Math.abs(dt.getRightEncRate()) <= Robot.getConst("Maximum Velocity When Stop", 1);
 	}
 
 	/**

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java

@@ -84,7 +84,9 @@ protected void execute() {
 	 */
 	@Override
 	protected boolean isFinished() {
-		return turnController.onTarget();
+		return turnController.onTarget()
+				&& Math.abs(dt.getLeftEncRate()) <= Robot.getConst("Maximum Velocity When Stop", 1)
+				&& Math.abs(dt.getRightEncRate()) <= Robot.getConst("Maximum Velocity When Stop", 1);
 	}
 
 	/**

Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java

@@ -34,6 +34,8 @@ public class Drivetrain extends Subsystem implements DrivetrainInterface {
 	private final WPI_VictorSPX dtRightSlave = RobotMap.dtRightSlave;
 	private final Encoder leftEncDist = RobotMap.leftEncDist;
 	private final Encoder rightEncDist = RobotMap.rightEncDist;
+	private final Encoder leftEncRate = RobotMap.leftEncRate;
+	private final Encoder rightEncRate = RobotMap.rightEncRate;
 	private final PIDSourceAverage distEncAvg = RobotMap.distEncAvg;
 	private final SpeedControllerGroup dtLeft = RobotMap.dtLeft;
 	private final SpeedControllerGroup dtRight = RobotMap.dtRight;
@@ -49,6 +51,24 @@ public void initDefaultCommand() {
 		setDefaultCommand(new TeleopDrive());
 	}
 
+	/**
+	 * Returns the getRate() of the left encoder
+	 * 
+	 * @return the rate of the left encoder
+	 */
+	public double getLeftEncRate() {
+		return leftEncRate.getRate();
+	}
+
+	/**
+	 * Returns the getRate() of the right encoder
+	 * 
+	 * @return the rate of the right encoder
+	 */
+	public double getRightEncRate() {
+		return rightEncRate.getRate();
+	}
+
 	/**
 	 * Sets the left side of the drivetrain to use the talons' pids to run at the
 	 * specified speed.

Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java

@@ -4,6 +4,20 @@ public interface DrivetrainInterface {
 
 	public void initDefaultCommand();
 
+	/**
+	 * Returns the getRate() of the left encoder
+	 * 
+	 * @return the rate of the left encoder
+	 */
+	public double getLeftEncRate();
+
+	/**
+	 * Returns the getRate() of the right encoder
+	 * 
+	 * @return the rate of the right encoder
+	 */
+	public double getRightEncRate();
+
 	/**
 	 * Drives based on joystick input and SmartDashboard values
 	 */