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();
}
public void setStraightProfile(MotionProfile profile) {
straightController.setProfile(profile);
}
public void resetControllers() {
straightController.setProfile(profile);
}
public boolean onTarget() {
return straightController.onTarget() && turnController.onTarget();
}
public void setTurnGoal(double angle) {
straightController.disable();
lastStraight = 0;
turnController.setGoal(angle);
turnController.enable();
}
public void updatePositionGoal(double distance) {
straightController.setGoalRaw(distance);
}
public void disableControllers() {
straightController.disable();
turnController.disable();
setLeftRightPower(0, 0);
}
public void shift(boolean highGear) {
isHighGear = highGear;
shifter.set(!isHighGear);
straightController.setGains(highGear ? highStraightGains : lowStraightGains);
turnController.setGains(highGear ? highTurnGains : lowTurnGains);
}