-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathPIDController.java
56 lines (47 loc) · 2.06 KB
/
PIDController.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
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
package frc.molib;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpiutil.math.MathUtil;
/**
* A wrapper class on the WPI PIDController
* <p>Adds an enabled flag and restricted output range.</p>
*/
public class PIDController extends edu.wpi.first.wpilibj.controller.PIDController {
private boolean enabled = false;
private double minLimit = Double.NEGATIVE_INFINITY;
private double maxLimit = Double.POSITIVE_INFINITY;
/**
* Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of 0.02 seconds.
* @param Kp The proportional coefficient
* @param Ki The integral coefficient
* @param Kd The derivative coefficient
*/
public PIDController(double Kp, double Ki, double Kd) { super(Kp, Ki, Kd); }
/**
* Allocates a PIDController with the given constants for Kp, Ki, and Kd.
* @param Kp The proportional coefficient
* @param Ki The integral coefficient
* @param Kd The derivative coefficient
* @param period The period between controller updates in seconds.
*/
public PIDController(double Kp, double Ki, double Kd, double period) { super(Kp, Ki, Kd, period); }
public boolean isEnabled() { return enabled; }
public void enable() { enabled = true; }
public void disable() { enabled = false; }
public void configOutputRange(double min, double max) {
minLimit = min;
maxLimit = max;
}
@Override
public double calculate(double measurement) { return MathUtil.clamp(super.calculate(measurement), minLimit, maxLimit); }
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("MOLib PIDController");
builder.setSafeState(() -> enabled = false);
builder.addDoubleProperty("P", this::getP, this::setP);
builder.addDoubleProperty("I", this::getI, this::setI);
builder.addDoubleProperty("D", this::getD, this::setD);
builder.addBooleanProperty("Enabled", this::isEnabled, value -> enabled = value);
builder.addDoubleProperty("Setpoint", this::getSetpoint, this::setSetpoint);
builder.addBooleanProperty("On Target", this::atSetpoint, null);
}
}