public void exec() {
XYPair curPos = robot().GetSensorCore().odometry.GetFieldOrientedPosition();
double[] output =
pidArray.calculate(
target.toArray(),
curPos.toArray(),
translationPositionP.get(),
translationPositionI.get(),
translationPositionD.get());
double rotationalForce = headingMaintainer.MaintainHeading();
robot()
.GetDriveCore()
.mecanumDrive_Cartesian(
output[0], output[1], rotationalForce, robot().GetSensorCore().getCurrentHeading());
}
public void init() {
headingMaintainer.SetSetpoint(robot().GetSensorCore().getCurrentHeading());
headingMaintainer.init();
pidArray.reset();
}