more kF implementation, PID input/output ranges, max high/low speed

implementation

- RobotMap:
set both VelocityPIDControllers' input ranges to +- Max High Speed,
output ranges are +-1, and initial kF constants based on Max Low Speed
(for more accuracy in Auto); set max output of robotDrive to Max High
Speed
- DT/DTInterface: new method getCurrentMaxSpeed() returns max speed
based on if robot is in high or low gear; new method
resetVelocityPIDkFConsts() that resets kF constants of both VPIDs based
on return of getCurrentMaxSpeed and returns new kF value; updated method
updatePidConstants() to call resetVelocityPIDkFConsts() instead of just
passing in 1/Max Speed
- PIDMove/Turn commands: when determining kF values for move/turn PIDs,
called getCurrentMaxSpeed() instead of SD value Max Speed; set
moveController input range to +- Max High Speed
- ShiftHigh/LowGear commands: called resetVelocityPIDkFConsts() after SD
value reset
- NOTE: all Max Speeds should be in inches/per!!!

Author: lhmcgann

Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java

@@ -113,9 +113,10 @@ public RobotMap() {
 
 		leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 1),
 				Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0),
-				1 / Robot.getConst("MaxSpeed", 204), leftEncRate, dtLeft);
+				1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft);
 		leftVelocityController.enable();
-		leftVelocityController.setInputRange(-Robot.getConst("Max Speed", 204), Robot.getConst("Max Speed", 204));
+		leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
+				Robot.getConst("Max High Speed", 204));
 		leftVelocityController.setOutputRange(-1.0, 1.0);
 		leftVelocityController.setContinuous(false);
 		leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2));
@@ -134,15 +135,16 @@ public RobotMap() {
 
 		rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 1),
 				Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0),
-				1 / Robot.getConst("MaxSpeed", 204), rightEncRate, dtRight);
+				1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
 		rightVelocityController.enable();
-		rightVelocityController.setInputRange(-Robot.getConst("Max Speed", 204), Robot.getConst("Max Speed", 204));
+		rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
+				Robot.getConst("Max High Speed", 204));
 		rightVelocityController.setOutputRange(-1.0, 1.0);
 		rightVelocityController.setContinuous(false);
 		rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
 
 		robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
-		robotDrive.setMaxOutput(Robot.getConst("Max Speed", 204));
+		robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
 
 		distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
 		fancyGyro = new AHRS(SerialPort.Port.kMXP);

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

@@ -23,7 +23,7 @@ public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) {
 		target = targ;
 		this.dt = dt;
 		requires(Robot.dt);
-		double kf = 1 / (Robot.getConst("Max Speed", 204) * Robot.getConst("Default PID Update Time", 0.05));
+		double kf = 1 / (dt.getCurrentMaxSpeed() * Robot.getConst("Default PID Update Time", 0.05));
 		moveController = new PIDController(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0),
 				Robot.getConst("MovekD", 0), kf, avg, this);
 	}
@@ -33,7 +33,7 @@ public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) {
 	public void initialize() {
 		dt.resetDistEncs();
 		// input is in inches
-		moveController.setInputRange(-Double.MAX_VALUE, Double.MAX_VALUE);
+		moveController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204));
 		// output in "motor units" (arcade and tank only accept values [-1, 1]
 		moveController.setOutputRange(-1.0, 1.0);
 		moveController.setContinuous(false);

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

@@ -26,7 +26,7 @@ public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) {
 		requires(Robot.dt);
 		// calculates the maximum turning speed in degrees/sec based on the max linear
 		// speed in inches/s and the distance (inches) between sides of the DT
-		double maxTurnSpeed = Robot.getConst("Max Speed", 204) * 360
+		double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360
 				/ (Math.PI * Robot.getConst("Distance Between Wheels", 26.25));
 		double kf = 1 / (maxTurnSpeed * Robot.getConst("Default PID Update Time", 0.05));
 		turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0),

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

@@ -21,5 +21,6 @@ public ShiftHighGear() {
 	protected void initialize() {
 		Robot.dt.shiftGears(true);
 		SmartDashboard.putBoolean("High Gear", true);
+		Robot.dt.resetVelocityPIDkFConsts();
 	}
 }

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

@@ -21,5 +21,6 @@ public ShiftLowGear() {
 	protected void initialize() {
 		Robot.dt.shiftGears(false);
 		SmartDashboard.putBoolean("High Gear", false);
+		Robot.dt.resetVelocityPIDkFConsts();
 	}
 }

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

@@ -111,10 +111,11 @@ public double getDtRightSpeed() {
 	 */
 	@Override
 	public void updatePidConstants() {
-		leftVelocityController.setPID(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0),
-				Robot.getConst("MoveLeftkD", 0), 1 / Robot.getConst("MaxSpeed", 17));
-		rightVelocityController.setPID(Robot.getConst("MoveRightkP", 1), Robot.getConst("MoveRightkI", 0),
-				Robot.getConst("MoveRightkD", 0), 1 / Robot.getConst("MaxSpeed", 17));
+		leftVelocityController.setPID(Robot.getConst("VelocityLeftkP", 1), Robot.getConst("VelocityLeftkI", 0),
+				Robot.getConst("VelocityLeftkD", 0));
+		rightVelocityController.setPID(Robot.getConst("VelocityRightkP", 1), Robot.getConst("VelocityRightkI", 0),
+				Robot.getConst("VelocityRightkD", 0));
+		resetVelocityPIDkFConsts();
 	}
 
 	/**
@@ -209,7 +210,7 @@ public double getRightEncDist() {
 	}
 
 	/**
-	 * Activates the solenoid to push the drivetrain into high or low gear
+	 * Activates the solenoid to push the drivetrain into high or low gear.
 	 * 
 	 * @param highGear
 	 *            If the solenoid is to be pushed into high gear (true, kForward) or
@@ -231,4 +232,35 @@ public void shiftGears(boolean highGear) {
 	public void shiftGearSolenoidOff() {
 		dtGear.set(DoubleSolenoid.Value.kOff);
 	}
+
+	/**
+	 * Reset the kf constants for both VelocityPIDControllers based on current DT
+	 * gearing (high or low gear).
+	 * 
+	 * @param newKF
+	 *            the new kF constant based on high and low gear max speeds; should
+	 *            be 1 / max speed
+	 * @return the new kF value as 1 / correct max speed
+	 */
+	@Override
+	public double resetVelocityPIDkFConsts() {
+		double newKF = 1 / getCurrentMaxSpeed();
+		leftVelocityController.setF(newKF);
+		rightVelocityController.setF(newKF);
+		return newKF;
+	}
+
+	/**
+	 * Gets the current max speed of the DT based on gearing (high or low gear)
+	 * 
+	 * @return the current max speed of the DT in inches/second
+	 */
+	@Override
+	public double getCurrentMaxSpeed() {
+		if (Robot.getBool("High Gear", true)) {
+			return Robot.getConst("Max High Speed", 204);
+		} else {
+			return Robot.getConst("Max Low Speed", 84);
+		}
+	}
 }

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

@@ -124,4 +124,22 @@ public interface DrivetrainInterface {
 	 * Stops the solenoid that pushes the drivetrain into low or high gear
 	 */
 	public void shiftGearSolenoidOff();
+
+	/**
+	 * Reset the kf constants for both VelocityPIDControllers based on current DT
+	 * gearing (high or low gear).
+	 * 
+	 * @param newKF
+	 *            the new kF constant based on high and low gear max speeds; should
+	 *            be 1 / max speed
+	 * @return the new kF value as 1 / correct max speed
+	 */
+	public double resetVelocityPIDkFConsts();
+
+	/**
+	 * Gets the current max speed of the DT based on gearing (high or low gear)
+	 * 
+	 * @return the current max speed of the DT in inches/second
+	 */
+	public double getCurrentMaxSpeed();
 }