summaryrefslogtreecommitdiff
path: root/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java
diff options
context:
space:
mode:
authorLuke Shumaker <lukeshu@sbcglobal.net>2015-02-16 17:40:02 -0500
committerLuke Shumaker <lukeshu@sbcglobal.net>2015-02-16 17:40:02 -0500
commit74b7077270fdd9d4d7a6eca0bb7011283ae999a8 (patch)
tree1776cc38d7e64bb980df5436655aeaa37dd27b30 /src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java
initial commit
Diffstat (limited to 'src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java')
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java38
1 files changed, 38 insertions, 0 deletions
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);
+ }
+}