From 74b7077270fdd9d4d7a6eca0bb7011283ae999a8 Mon Sep 17 00:00:00 2001 From: Luke Shumaker Date: Mon, 16 Feb 2015 17:40:02 -0500 Subject: initial commit --- .../team4272/robotlib/LimitSwitchedPIDOutput.java | 38 ++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java (limited to 'src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java') diff --git a/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java new file mode 100644 index 0000000..73073c2 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java @@ -0,0 +1,38 @@ +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.PIDOutput; + +public class LimitSwitchedPIDOutput implements PIDOutput { + private final PIDOutput out; + private final DigitalInput forward; + private final DigitalInput backward; + private final boolean i_dont_know_which_way_is_which; + + public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward, boolean i_dont_know_which_way_is_which) { + this.out = out; + this.forward = forward; + this.backward = backward; + this.i_dont_know_which_way_is_which = i_dont_know_which_way_is_which; + } + + public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward) { + this(out, forward,backward, false); + } + + public void pidWrite(double v) { + if (i_dont_know_which_way_is_which) { + if (forward.get() || backward.get()) { + v = 0; + } + } else { + if (forward.get()) { + v = Math.min(v, 0); + } + if (backward.get()) { + v = Math.max(v, 0); + } + } + out.pidWrite(v); + } +} -- cgit v1.2.3-54-g00ecf