diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robotlib/PIDController.java')
| -rw-r--r-- | src/org/usfirst/frc/team4272/robotlib/PIDController.java | 153 |
1 files changed, 118 insertions, 35 deletions
diff --git a/src/org/usfirst/frc/team4272/robotlib/PIDController.java b/src/org/usfirst/frc/team4272/robotlib/PIDController.java index 6650ad4..14c26ce 100644 --- a/src/org/usfirst/frc/team4272/robotlib/PIDController.java +++ b/src/org/usfirst/frc/team4272/robotlib/PIDController.java @@ -1,4 +1,9 @@ /** + * Copyright (c) FIRST 2008-2012. All Rights Reserved. + * Open Source Software - may be modified and shared by FRC teams. The code + * must be accompanied by the FIRST BSD license file in the root directory of + * the project. + * * Copyright (c) 2011-2012, 2015-2016 Luke Shumaker * All rights reserved. * @@ -34,7 +39,13 @@ import edu.wpi.first.wpilibj.SpeedController; //import edu.wpi.first.wpilibj.PIDController; /** - * A variant of edu.wpi.first.wpilibj.PIDController that also: + * An enhanced variant of {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController} + * (implements a PID Control Loop). + * + * It is enhanced from + * {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController} + * in that it that also: + * * <ul> * <li> implements SpeedController (and therefore PIDOutput) * <li> has auto-enable/disable functionality to avoid "locking" the @@ -43,77 +54,149 @@ import edu.wpi.first.wpilibj.SpeedController; * @author Luke Shumaker <lukeshu@sbcglobal.net> */ public class PIDController extends edu.wpi.first.wpilibj.PIDController implements SpeedController { - private boolean autodisable = false; - /** - * Default to true, so if we don't use autodisable, we don't enable when we - * shouldn't, as we will think it's already enabled. - */ - private boolean enabled = true; + private boolean NaNDisabled = false; + private boolean inverted = false; public final PIDSource source; public final PIDOutput output; - /* Constructors *******************************************************/ - - public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) { - this(Kp, Ki, Kd, source, output, kDefaultPeriod, false); + /** + * Allocate a PID object with the given constants for P, I, D, and F + *$ + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + * @param Kf the feed forward term + * @param source The PIDSource object that is used to get values + * @param output The PIDOutput object that is set to the output percentage + * @param period the loop time for doing calculations. This particularly + * effects calculations of the integral and differential terms. The + * default is 50ms. + */ + public PIDController(double Kp, double Ki, double Kd, double Kf, + PIDSource source, PIDOutput output, + double period) { + super(Kp, Ki, Kd, Kf, source, output, period); + this.source = source; + this.output = output; } - public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) { - this(Kp, Ki, Kd, source, output, period, false); + + /* Convenience constructors *******************************************/ + /* `Kf`, and `period` are optional */ + + /** + * Convenience constructor with {@code Kf} defaulting to + * {@code 0.0}. + */ + public PIDController(double Kp, double Ki, double Kd, + PIDSource source, PIDOutput output, + double period) { + this(Kp, Ki, Kd, 0.0, source, output, period); } - public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, boolean autodisable) { - this(Kp, Ki, Kd, source, output, kDefaultPeriod, autodisable); + + /** + * Convenience constructor with {@code period} defaulting to + * {@link #kDefaultPeriod}. + */ + public PIDController(double Kp, double Ki, double Kd, double Kf, + PIDSource source, PIDOutput output) { + this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod); } - public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period, boolean autodisable) { - super(Kp, Ki, Kd, source, output, period); - this.autodisable = autodisable; - this.enabled = this.isEnabled(); - this.source = source; - this.output = output; + + /** + * Convenience constructor with {@code Kf} defaulting to + * {@code 0.0}, and {@code period} defaulting to + * {@link #kDefaultPeriod}. + */ + public PIDController(double Kp, double Ki, double Kd, + PIDSource source, PIDOutput output) { + this(Kp, Ki, Kd, 0.0, source, output, kDefaultPeriod); } - /* Mimic the four PIDController constructors **************************/ + /* Override to auto-disable if setpoint is NaN, and invert ************/ - public synchronized void setSetpoint(double output) { - if ((output == 0) && autodisable) { - disable(); - enabled = false; + /** + * {@inheritDoc} + */ + @Override + public synchronized void setSetpoint(double setpoint) { + if (Double.isNaN(setpoint)) { + NaNDisabled = true; + super.disable(); } else { - if (!enabled) { + if (NaNDisabled && !isEnabled()) { enable(); - enabled = true; } - super.setSetpoint(output); + super.setSetpoint((inverted ? -1 : 1) * setpoint); } } + /** + * {@inheritDoc} + */ + @Override + public synchronized void enable() { + NaNDisabled = false; + super.enable(); + } + + /** + * {@inheritDoc} + */ + @Override + public synchronized void disable() { + NaNDisabled = false; + super.disable(); + } + /* Implement PIDOutput (a parent of SpeedController) *******************/ + /** + * {@inheritDoc} + */ + @Override public void pidWrite(double output) { setSetpoint(output); } /* Implement SpeedController *******************************************/ + /** + * {@inheritDoc} + */ + @Override public void set(double output) { setSetpoint(output); } /** - * Don't use this; it is leaking up from CANJaguar + * Don't use this; it is leaking up from + * {@link edu.wpi.first.wpilibj.CANJaguar CANJaguar} through + * {@link edu.wpi.first.wpilibj.SpeedController SpeedController} + * * @param output * @param syncGroup + * + * @deprecated Don't use this, it is leaking up from + * {@link edu.wpi.first.wpilibj.CANJaguar CANJaguar} */ + @Override public void set(double output, byte syncGroup) { - setSetpoint(output); + set(output); } + /** + * {@inheritDoc} + */ + @Override public void setInverted(boolean isInverted) { - // TODO Auto-generated method stub - + inverted = isInverted; } + /** + * {@inheritDoc} + */ + @Override public boolean getInverted() { - // TODO Auto-generated method stub - return false; + return inverted; } } |