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