src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

/**
 * TODO: copyright
 *
 * @author Luke Shumaker <lukeshu@sbcglobal.net>
 */
package org.usfirst.frc.team4272.robotlib;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.PIDOutput;

/**
 * TODO: Write JavaDocs
 */
public class LimitSwitchedPIDOutput implements PIDOutput {
	private final PIDOutput out;
	private final DigitalInput forward;
	private final DigitalInput bakward;
	private final boolean for_pressed;
	private final boolean bak_pressed;

	public LimitSwitchedPIDOutput(PIDOutput out,
			DigitalInput forward, boolean for_pressed,
			DigitalInput backward, boolean back_pressed) {
		this.out = out;
		this.forward = forward;
		this.bakward = backward;
		this.for_pressed = for_pressed;
		this.bak_pressed = back_pressed;
	}

	public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward) {
		this(out, forward, true, backward, true);
	}

	public void pidWrite(double v) {
		if (forward.get() == for_pressed) { v = Math.min(v, 0); }
		if (bakward.get() == bak_pressed) { v = Math.max(v, 0); }
		out.pidWrite(v);
	}
}