diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java')
-rw-r--r-- | src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java | 30 |
1 files changed, 12 insertions, 18 deletions
diff --git a/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java index 73073c2..aa3aeaf 100644 --- a/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java +++ b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java @@ -6,33 +6,27 @@ 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; + private final DigitalInput bakward; + private final boolean for_pressed; + private final boolean bak_pressed; - public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward, boolean i_dont_know_which_way_is_which) { + public LimitSwitchedPIDOutput(PIDOutput out, + DigitalInput forward, boolean for_pressed, + DigitalInput backward, boolean back_pressed) { this.out = out; this.forward = forward; - this.backward = backward; - this.i_dont_know_which_way_is_which = i_dont_know_which_way_is_which; + this.bakward = backward; + this.for_pressed = for_pressed; + this.bak_pressed = back_pressed; } public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward) { - this(out, forward,backward, false); + this(out, forward, true, backward, true); } 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); - } - } + if (forward.get() == for_pressed) { v = Math.min(v, 0); } + if (bakward.get() == bak_pressed) { v = Math.max(v, 0); } out.pidWrite(v); } } |