public void initTable(ITable table) {
if (this.table != null) {
this.table.removeTableListener(listener);
}
this.table = table;
if (table != null) {
table.putNumber("p", gains.getP());
table.putNumber("i", gains.getI());
table.putNumber("d", gains.getD());
table.putNumber("f", gains.getF());
table.putNumber("goal", goal);
table.putNumber("source", source.get());
table.putBoolean("enabled", isEnabled());
table.addTableListener(listener, false);
}
}
public void update() {
lastSource = source.get();
double out = 0;
double error = goal - lastSource;
double p = gains.getP() * error;
if (Math.abs(error) < minIError) errorSum += error;
double i = gains.getI() * errorSum;
double dError = error - lastError;
double d = gains.getD() * dError;
lastError = error;
double ff = gains.getF() * goal;
if (enabled) {
out = ff + p + i + d;
output.set(out);
lastOut = out;
}
lastDeltaError = dError;
SmartDashboard.putData(this);
System.out.println(enabled + " " + error + " " + out);
}
public void valueChanged(ITable table, String key, Object value, boolean isNew) {
if (key.equals("p") || key.equals("i") || key.equals("d") || key.equals("f")) {
if (gains.getP() != table.getNumber("p", 0.0)
|| gains.getI() != table.getNumber("i", 0.0)
|| gains.getD() != table.getNumber("d", 0.0)) {
System.out.println("Got new PID gains!");
gains.set(
table.getNumber("p", 0.0), table.getNumber("i", 0.0), table.getNumber("d", 0.0));
}
} else if (key.equals("setpoint")) {
if (goal != ((Double) value).doubleValue()) {
setGoal(((Double) value).doubleValue());
}
} else if (key.equals("enabled")) {
if (isEnabled() != ((Boolean) value).booleanValue()) {
if (((Boolean) value).booleanValue()) {
enable();
} else {
disable();
}
}
}
}