public void setPositionGoal(double distance, double angle, double speed) {
profile.setMaxVelocity(speed);
profile.setTimeToMaxV(.2);
straightController.setGoal(distance);
straightController.enable();
turnController.setGoal(angle);
turnController.enable();
}
public void setSpeedGoal(double speed, double angle) {
profile.setMaxVelocity(Math.abs(speed));
profile.setTimeToMaxV(0.001);
straightController.setGoal(speed < 0 ? -1000 : 1000);
straightController.enable();
turnController.setGoal(angle);
turnController.enable();
}