Calling this method does not alter the arm's motion; it simply updates a value used for + * telemetry. + * + * @param goal the arm's goal. + */ public void setGoal(ArmState goal) { this.goal = goal; } + /** + * Returns the arm's setpoint. + * + * @return the arm's setpoint. + */ public ArmState getSetpoint() { - return setpoint; + return this.setpoint; } - public boolean atSetpoint() { - return getMeasuredState().at(setpoint); - } - - private void setSetpoint(ArmState setpoint) { + /** + * Sets the arm's setpoint. + * + *
Calling this method does not alter the arm's motion; it simply updates a value used for + * telemetry. + * + * @param setpoint the arm's setpoint. + */ + public void setSetpoint(ArmState setpoint) { this.setpoint = setpoint; + } + /** + * Applies a setpoint to the arm's controllers. + * + *
Calling this method alters the arm's motion. + * + * @param setpoint the arm's setpoint. + */ + public void applySetpoint(ArmState setpoint) { shoulderMotor.setSetpoint(setpoint.shoulder().position, setpoint.shoulder().velocity); wristMotor.setSetpoint(setpoint.wrist().position, setpoint.wrist().velocity); } - private MoveShoulderCommand moveShoulder(ArmState goal) { + /** + * Returns a command that moves the shoulder to a goal's shoulder position. + * + * @param goal the goal position to move to. + * @return a command that moves the shoulder to a goal's shoulder position. + */ + private Command shoulderTo(ArmState goal) { return new MoveShoulderCommand(goal); } - public MoveWristCommand moveWrist(ArmState goal) { + /** + * Returns a command that moves the wrist to a goal's wrist position. + * + * @param goal the goal position to move to. + * @return a command that moves the wrist to a goal's wrist position. + */ + public Command wristTo(ArmState goal) { return new MoveWristCommand(goal); } - public Command moveShoulderThenWrist(ArmState goal) { - return moveShoulder(goal).andThen(moveWrist(goal)); - } - - public Command moveWristThenShoulder(ArmState goal) { - return moveWrist(goal).andThen(moveShoulder(goal)); + /** + * Returns a command that moves the arm to the amp position. + * + * @return a comamnd that moves the arm to the amp position. + */ + public Command amp() { + return shoulderTo(ArmState.AMP).andThen(wristTo(ArmState.AMP)); } - public Command amp() { - return moveShoulderThenWrist(ArmState.AMP); + /** + * Returns a command that moves the arm to the stow position. + * + * @return a command that moves the arm to the stow position. + */ + public Command stow() { + return wristTo(ArmState.STOW).andThen(shoulderTo(ArmState.STOW)); } + /** + * Returns a command that moves the arm to the stow position and resets the position of the arm. + * + *
When the limit switch detects that the arm is in the stow position, the arm position is + * reset to be equal to the stow position. + * + * @return a command that moves the arm to the stow position and resets the position of the arm. + */ public Command home() { - return moveWrist(ArmState.STOW) - .andThen(moveShoulder(ArmState.STOW).until(() -> limitSwitchValues.isPressed)) + return wristTo(ArmState.STOW) + .andThen(shoulderTo(ArmState.STOW).until(() -> limitSwitchValues.isPressed)) .finallyDo( interrupted -> { if (!interrupted) { setPosition(ArmState.STOW); } - }) - .withTimeout(3.0); + }); } } diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java index a4e5cf5..7fbd119 100644 --- a/src/main/java/frc/robot/arm/ArmState.java +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -152,29 +152,4 @@ public ArmState nextWristSetpoint(ArmState goal) { WristMotorConstants.MOTION_PROFILE.calculate( RobotConstants.PERIODIC_DURATION, this.wrist, goal.wrist)); } - - /** - * Returns the next overall setpoint of the arm's movement. - * - * @param goal the arm's goal state. - * @return the next overall setpoint. - */ - public ArmState nextSetpoint(ArmState goal) { - boolean shooterOnBottom = Rotation2d.fromRotations(wrist.position).getDegrees() < 0.0; - boolean shooterNeedsToBeOnTop = - Rotation2d.fromRotations(goal.wrist.position).getDegrees() > 0.0; - boolean shooterOnWrongSide = shooterOnBottom && shooterNeedsToBeOnTop; - - if (shooterOnWrongSide && !atWristGoal(goal)) { - return nextWristSetpoint(goal); - } - - if (!atShoulderGoal(goal)) { - return nextShoulderSetpoint(goal); - } else if (!atWristGoal(goal)) { - return nextWristSetpoint(goal); - } - - return this; - } } diff --git a/src/main/java/frc/robot/arm/MoveShoulderCommand.java b/src/main/java/frc/robot/arm/MoveShoulderCommand.java index 0793a43..f5d1c20 100644 --- a/src/main/java/frc/robot/arm/MoveShoulderCommand.java +++ b/src/main/java/frc/robot/arm/MoveShoulderCommand.java @@ -24,13 +24,22 @@ public void initialize() { before = arm.getMeasuredState().position(); arm.setGoal(shoulderGoal.withWrist(before.wrist())); + arm.setSetpoint(before); + } + + @Override + public void execute() { + ArmState nextSetpoint = arm.getSetpoint().nextShoulderSetpoint(shoulderGoal); + + arm.setSetpoint(nextSetpoint); + arm.applySetpoint(nextSetpoint); } @Override public void end(boolean interrupted) { ArmState after = arm.getMeasuredState().position(); - // Since only the shoulder should have moved, assume that the wrist is in the same position as + // Since only the shoulder *should* have moved, assume that the wrist is in the same position as // it was at the start of the command arm.setPosition(after.withWrist(before.wrist())); } diff --git a/src/main/java/frc/robot/arm/MoveWristCommand.java b/src/main/java/frc/robot/arm/MoveWristCommand.java index f6fcb00..f47ee55 100644 --- a/src/main/java/frc/robot/arm/MoveWristCommand.java +++ b/src/main/java/frc/robot/arm/MoveWristCommand.java @@ -24,13 +24,22 @@ public void initialize() { before = arm.getMeasuredState().position(); arm.setGoal(wristGoal.withShoulder(before.shoulder())); + arm.setSetpoint(before); + } + + @Override + public void execute() { + ArmState nextSetpoint = arm.getSetpoint().nextWristSetpoint(wristGoal); + + arm.setSetpoint(nextSetpoint); + arm.applySetpoint(nextSetpoint); } @Override public void end(boolean interrupted) { ArmState after = arm.getMeasuredState().position(); - // Since only the wrist should have moved, assume that the shoulder is in the same position as + // Since only the wrist *should* have moved, assume that the shoulder is in the same position as // it was at the start of the command arm.setPosition(after.withShoulder(before.shoulder())); } diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index b02799a..5283e0d 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -135,8 +135,7 @@ public Command readyIntake() { double seconds = 3.0; return Commands.parallel( - Commands.waitUntil(intake::isNotStowed) - .andThen(arm.moveShoulderThenWrist(ArmState.INTAKE)), + Commands.waitUntil(intake::isNotStowed).andThen(arm.wristTo(ArmState.INTAKE)), intake.out()) .withTimeout(seconds); } @@ -149,10 +148,8 @@ public Command stow() { double seconds = 2.0; return Commands.parallel( - arm.moveWristThenShoulder(ArmState.STOW), - Commands.waitUntil(() -> arm.getMeasuredState().at(ArmState.STOW)) - .withTimeout(2.0) - .andThen(intake.in())) + arm.stow(), + Commands.waitUntil(() -> arm.at(ArmState.STOW)).withTimeout(2.0).andThen(intake.in())) .withTimeout(seconds); } @@ -160,7 +157,7 @@ public Command readyShoot() { double seconds = 3.0; return Commands.parallel( - Commands.waitUntil(intake::isNotStowed).andThen(arm.moveWrist(ArmState.SHOOT)), + Commands.waitUntil(intake::isNotStowed).andThen(arm.wristTo(ArmState.SHOOT)), intake.out()) .withTimeout(seconds); }