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

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