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