public void init() {
headingMaintainer.SetSetpoint(robot().GetSensorCore().getCurrentHeading());
headingMaintainer.init();
pidArray.reset();
}
/** @param target */
public void setSetpoint(XYPair target) {
pidArray.reset();
this.target = target;
}