diff options
Diffstat (limited to 'src/org/mckenzierobotics')
-rw-r--r-- | src/org/mckenzierobotics/lib/robot/FeedForward.java | 2 | ||||
-rw-r--r-- | src/org/mckenzierobotics/lib/robot/PIDController.java | 14 | ||||
-rw-r--r-- | src/org/mckenzierobotics/lib/robot/RollingAvg.java | 19 |
3 files changed, 30 insertions, 5 deletions
diff --git a/src/org/mckenzierobotics/lib/robot/FeedForward.java b/src/org/mckenzierobotics/lib/robot/FeedForward.java index a275366..a18dccc 100644 --- a/src/org/mckenzierobotics/lib/robot/FeedForward.java +++ b/src/org/mckenzierobotics/lib/robot/FeedForward.java @@ -64,7 +64,7 @@ public abstract class FeedForward implements PIDOutput { } public void update(double pidResult) { - if (pid.isEnable()) { + if (pid.isEnabled()) { double ffResult = calculate(pid.getSetpoint()); pidOutput.pidWrite(ffResult+pidResult); } else { diff --git a/src/org/mckenzierobotics/lib/robot/PIDController.java b/src/org/mckenzierobotics/lib/robot/PIDController.java index dc2301a..e2f8087 100644 --- a/src/org/mckenzierobotics/lib/robot/PIDController.java +++ b/src/org/mckenzierobotics/lib/robot/PIDController.java @@ -60,14 +60,14 @@ public class PIDController extends edu.wpi.first.wpilibj.PIDController implement public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, boolean autodisable) { super(Kp, Ki, Kd, source, output); this.autodisable = autodisable; - enabled = this.isEnable(); + enabled = this.isEnabled(); this.source = source; this.output = output; } 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; - enabled = this.isEnable(); + enabled = this.isEnabled(); this.source = source; this.output = output; } @@ -101,4 +101,14 @@ public class PIDController extends edu.wpi.first.wpilibj.PIDController implement public void set(double output, byte syncGroup) { setSetpoint(output); } + + public void setInverted(boolean isInverted) { + // TODO Auto-generated method stub + + } + + public boolean getInverted() { + // TODO Auto-generated method stub + return false; + } } diff --git a/src/org/mckenzierobotics/lib/robot/RollingAvg.java b/src/org/mckenzierobotics/lib/robot/RollingAvg.java index fcbf8e4..d73f64e 100644 --- a/src/org/mckenzierobotics/lib/robot/RollingAvg.java +++ b/src/org/mckenzierobotics/lib/robot/RollingAvg.java @@ -30,6 +30,7 @@ package org.mckenzierobotics.lib.robot; import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.PIDOutput; /** @@ -65,11 +66,25 @@ public class RollingAvg implements PIDSource, PIDOutput { } public double pidGet() { - if (source!=null) return push(source.pidGet()); - return get(); + if (source!=null) + return push(source.pidGet()); + else + return get(); } public void pidWrite(double output) { push(output); } + + public void setPIDSourceType(PIDSourceType srcType) { + if (source!=null) + source.setPIDSourceType(srcType); + } + + public PIDSourceType getPIDSourceType() { + if (source!=null) + return source.getPIDSourceType(); + else + return null; + } } |