PID input/output range changes

set proper kF constants for move and turn PIDs as well
fixed inputRange and outputRange for each PID controller (move, turn,
leftV, rightV)
setMaxOutput of robotDrive to the robot's max speed (in inches)

Author: lhmcgann

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

@@ -111,14 +111,14 @@ public RobotMap() {
 		configSPX(dtLeftSlave);
 		dtLeft = new SpeedControllerGroup(dtLeftMaster, dtLeftSlave);
 
-		leftVelocityController = new VelocityPIDController(Robot.getConst("MoveLeftkP", 1),
-				Robot.getConst("MoveLeftkI", 0), Robot.getConst("MoveLeftkD", 0), 1 / Robot.getConst("MaxSpeed", 17),
-				leftEncRate, dtLeft);
+		leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 1),
+				Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0),
+				1 / Robot.getConst("MaxSpeed", 204), leftEncRate, dtLeft);
 		leftVelocityController.enable();
-		leftVelocityController.setInputRange(0, Double.MAX_VALUE);
+		leftVelocityController.setInputRange(-Robot.getConst("Max Speed", 204), Robot.getConst("Max Speed", 204));
 		leftVelocityController.setOutputRange(-1.0, 1.0);
 		leftVelocityController.setContinuous(false);
-		leftVelocityController.setAbsoluteTolerance(Robot.getConst("MoveToleranceLeft", 2));
+		leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2));
 
 		rightEncPort1 = new DigitalInput(getPort("1RightEnc", 2));
 		rightEncPort2 = new DigitalInput(getPort("2RightEnc", 3));
@@ -132,16 +132,17 @@ public RobotMap() {
 		configSPX(dtRightSlave);
 		dtRight = new SpeedControllerGroup(dtRightMaster, dtRightSlave);
 
-		rightVelocityController = new VelocityPIDController(Robot.getConst("MoveRightkP", 1),
-				Robot.getConst("MoveRightkI", 0), Robot.getConst("MoveRightkD", 0), 1 / Robot.getConst("MaxSpeed", 17),
-				rightEncRate, dtRight);
+		rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 1),
+				Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0),
+				1 / Robot.getConst("MaxSpeed", 204), rightEncRate, dtRight);
 		rightVelocityController.enable();
-		rightVelocityController.setInputRange(0, Double.MAX_VALUE);
+		rightVelocityController.setInputRange(-Robot.getConst("Max Speed", 204), Robot.getConst("Max Speed", 204));
 		rightVelocityController.setOutputRange(-1.0, 1.0);
 		rightVelocityController.setContinuous(false);
-		rightVelocityController.setAbsoluteTolerance(Robot.getConst("MoveToleranceRight", 2));
+		rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
 
 		robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
+		robotDrive.setMaxOutput(Robot.getConst("Max Speed", 204));
 
 		distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
 		fancyGyro = new AHRS(SerialPort.Port.kMXP);

Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java

@@ -37,7 +37,7 @@ public VelocityPIDController(double kp, double ki, double kd, double kf, PIDSour
 	 * Sets the target speed
 	 * 
 	 * @param speed
-	 *            the target speed [-1, 1]
+	 *            the target speed in inches/second
 	 */
 	@Override
 	public void pidWrite(double output) {
@@ -48,7 +48,7 @@ public void pidWrite(double output) {
 	 * Sets the target speed
 	 * 
 	 * @param speed
-	 *            the target speed [-1, 1]
+	 *            the target speed in inches/second
 	 */
 	@Override
 	public void set(double speed) {
@@ -68,24 +68,29 @@ public double get() {
 	}
 
 	/**
+	 * Invert this side of the DT (flip forwards and backwards).
 	 * 
-	 * */
+	 * @param isInverted
+	 *            invert this side of the DT or not
+	 */
 	@Override
 	public void setInverted(boolean isInverted) {
 		out.setInverted(isInverted);
 	}
 
 	/**
+	 * Get whether or not this side of the DT is inverted.
 	 * 
-	 * */
+	 * @return is this side of the DT inverted or not
+	 */
 	@Override
 	public boolean getInverted() {
 		return out.getInverted();
 	}
 
 	/**
-	 * 
-	 * */
+	 * Set the output to zero.
+	 */
 	@Override
 	public void stopMotor() {
 		out.stopMotor();

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

@@ -23,15 +23,18 @@ 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));
 		moveController = new PIDController(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0),
-				Robot.getConst("MovekD", 0), avg, this);
+				Robot.getConst("MovekD", 0), kf, avg, this);
 	}
 
 	// Called just before this Command runs the first time
 	@Override
 	public void initialize() {
 		dt.resetDistEncs();
-		moveController.setInputRange(0, Double.MAX_VALUE);
+		// input is in inches
+		moveController.setInputRange(-Double.MAX_VALUE, Double.MAX_VALUE);
+		// output in "motor units" (arcade and tank only accept values [-1, 1]
 		moveController.setOutputRange(-1.0, 1.0);
 		moveController.setContinuous(false);
 		moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));

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

@@ -24,16 +24,23 @@ public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) {
 		target = targ;
 		this.dt = dt;
 		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
+				/ (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),
-				Robot.getConst("TurnkD", 0), ahrs, this);
+				Robot.getConst("TurnkD", 0), kf, ahrs, this);
 	}
 
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
 		dt.resetAHRS();
 		turnController.disable();
+		// input is in degrees
 		turnController.setInputRange(-180, 180);
+		// output in "motor units" (arcade and tank only accept values [-1, 1]
 		turnController.setOutputRange(-1.0, 1.0);
 		turnController.setContinuous();
 		turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1));