initial commit

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);
+	}
+}