public void init() {
   headingMaintainer.SetSetpoint(robot().GetSensorCore().getCurrentHeading());
   headingMaintainer.init();
   pidArray.reset();
 }