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