Team 4911 Arm Tuning Mode

ArmCommand.java

import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ProxyCommand;

// NOTE: Modify whatever command class you currently have.
public final class ArmCommand extends CommandBase {
    private final InitializedListener listener;

    private ArmCommand(/* TODO: put your other constructor args here. */ InitializedListener listener) {
        // this is used to keep test mode updated with the most recently used arm position
        this.listener = listener;
    }
    
    @Override
    public void initialize() {
        // TODO: your implementation goes here

        // NOTE: be sure to do this at some point during initialization
        listener.onInitialize(this);
    }

    /** Used in tuning mode to move to the desired positions after modifying them. */
    private void rerunMoveToPosition() {
        // TODO: re-issue the movement calls
        // for us it looks like:
        // armSubsystem.moveWrist(desiredArmPosition.wristPosition.getValue());
        // armSubsystem.moveShoulder(desiredArmPosition.shoulderPosition.getValue());
    }

    /**
     * Creates a new command for moving the arm.
     */
    public static ArmCommand create(
            ArmSubsystem armSubsystem, ArmPositions desiredPosition) {
        return new ArmCommand(armSubsystem, desiredPosition, LISTENER);
    }

    private static final InitializedListener LISTENER = Constants.Arm.IS_TUNING_ENABLED ?
       new TestMode() : (command) -> {};

    private interface InitializedListener {
        void onInitialize(ArmCommand armCommand);
    }

    /**
     * How to use:
     * 1. enable via Constants
     * 2. enable robot and issue an arm command to be tuned
     * 3. in Shuffleboard, select the "ARM TUNING" tab
     * 4. click +/- Shuffleboard buttons until the position is correct
     * 5. that's it, the position is saved until robot preferences are cleared
     */
    private static class TestMode implements InitializedListener {
        private ArmCommand currentCommand = null;

        private TestMode() {
            ShuffleboardTab tab = Shuffleboard.getTab("ARM TUNING");
            tab.add("SHOULDER+", increaseShoulder());
            tab.add("SHOULDER-", decreaseShoulder());
            tab.add("WRIST+", increaseWrist());
            tab.add("WRIST-", decreaseWrist());
        }

        CommandBase increaseShoulder() {
            return new ProxyCommand(
                () -> increase(currentCommand, currentCommand.desiredPosition.shoulderPosition));
        }

        CommandBase decreaseShoulder() {
            return new ProxyCommand(
                () -> decrease(currentCommand, currentCommand.desiredPosition.shoulderPosition));
        }

        CommandBase increaseWrist() {
            return new ProxyCommand(
                () -> increase(currentCommand, currentCommand.desiredPosition.wristPosition));
        }

        CommandBase decreaseWrist() {
            return new ProxyCommand(
                () -> decrease(currentCommand, currentCommand.desiredPosition.wristPosition));
        }

        // NOTE: you may wish to change the increment amount, especially if your arm positions are not in degrees
        
        private CommandBase increase(ArmCommand currentCommand, DoublePreference doublePreference) {
            return Commands.runOnce(() -> {
                doublePreference.setValue(doublePreference.getValue() + 1);
                currentCommand.rerunMoveToPosition();
            });
        }

        private CommandBase decrease(ArmCommand currentCommand, DoublePreference doublePreference) {
            return Commands.runOnce(() -> {
                doublePreference.setValue(doublePreference.getValue() - 1);
                currentCommand.rerunMoveToPosition();
            });
        }

        @Override
        public void onInitialize(ArmCommand armCommand) {
            currentCommand = armCommand;
        }
    }
}

ArmPositions.java

public enum ArmPositions {
    STOWED(
        Constants.Arm.STOWED_SHOULDER,
        Constants.Arm.STOWED_WRIST),
    SCORE_L2(
        Constants.Arm.SCORE_L2_SHOULDER, 
        Constants.Arm.SCORE_L2_WRIST);
    // TODO: populate with the rest of your arm positions

    public final DoublePreference shoulderPosition;
    public final DoublePreference wristPosition;

    private ArmPositions(DoublePreference shoulderPosition, DoublePreference wristPosition) {
        this.shoulderPosition = shoulderPosition;
        this.wristPosition = wristPosition;
    }
}

Constants.java

public class Constants {
    public static class Arm {
        private Arm() {}

        // Toggle "Tuning Mode" on and off
        public static final boolean IS_TUNING_ENABLED = true;
        
        // NOTE: these positions will be overridden by values that have been set in Shuffleboard.
        // (See note in DoublePreference.java)
        public static final DoublePreference STOWED_WRIST =
            new DoublePreference("STOWED_WRIST", 325);
        public static final DoublePreference STOWED_SHOULDER =
            new DoublePreference("STOWED_SHOULDER", 38);
        public static final DoublePreference SCORE_L2_WRIST =
            new DoublePreference("SCORE_L2_WRIST", 211);
        public static final DoublePreference SCORE_L2_SHOULDER =
            new DoublePreference("SCORE_L2_SHOULDER", 93);
        // TODO: populate with the rest of your arm positions constant values
    }
}

DoublePreference.java

import edu.wpi.first.wpilibj.Preferences;

/** Convenience class for defining double constants that can be modified via ShuffleBoard. */
public final class DoublePreference {
    private final String key;
    private final double defaultValue;

    public DoublePreference(String key, double defaultValue) {
        this.key = key;
        this.defaultValue = defaultValue;
        // NOTE: disabling the line below will cause the getValue() to fall back to defaultValue
        Preferences.initDouble(key, defaultValue);
    }

    public double getValue() {
        return Preferences.getDouble(key, defaultValue);
    }

    public void setValue(double value) {
        Preferences.setDouble(key, value);
    }
}