blob: 73073c2ed80840fbfa6f476ffc8d52fba6f09907 (
plain)
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);
}
}
|