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

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