Add Elevator Testing

src/main/java/frc/lib/util/LoggedTunableNumber.java

@@ -0,0 +1,124 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.lib.util;
+
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+import frc.robot.Constants;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class LoggedTunableNumber implements DoubleSupplier {
+    private static final String tableKey = "TunableNumbers";
+
+    private final String key;
+    private boolean hasDefault = false;
+    private double defaultValue;
+    private LoggedDashboardNumber dashboardNumber;
+    private Map<Integer, Double> lastHasChangedValues = new HashMap<>();
+
+    /**
+     * Create a new LoggedTunableNumber
+     *
+     * @param dashboardKey Key on dashboard
+     */
+    public LoggedTunableNumber(String dashboardKey) {
+        this.key = tableKey + "/" + dashboardKey;
+    }
+
+    /**
+     * Create a new LoggedTunableNumber with the default value
+     *
+     * @param dashboardKey Key on dashboard
+     * @param defaultValue Default value
+     */
+    public LoggedTunableNumber(String dashboardKey, double defaultValue) {
+        this(dashboardKey);
+        initDefault(defaultValue);
+    }
+
+    /**
+     * Set the default value of the number. The default value can only be set once.
+     *
+     * @param defaultValue The default value
+     */
+    public void initDefault(double defaultValue) {
+        if (!hasDefault) {
+            hasDefault = true;
+            this.defaultValue = defaultValue;
+            if (Constants.tuningMode) {
+                dashboardNumber = new LoggedDashboardNumber(key, defaultValue);
+            }
+        }
+    }
+
+    /**
+     * Get the current value, from dashboard if available and in tuning mode.
+     *
+     * @return The current value
+     */
+    public double get() {
+        if (!hasDefault) {
+            return 0.0;
+        } else {
+            return Constants.tuningMode ? dashboardNumber.get() : defaultValue;
+        }
+    }
+
+    /**
+     * Checks whether the number has changed since our last check
+     *
+     * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
+     *        objects. Recommended approach is to pass the result of "hashCode()"
+     * @return True if the number has changed since the last time this method was called, false
+     *         otherwise.
+     */
+    public boolean hasChanged(int id) {
+        double currentValue = get();
+        Double lastValue = lastHasChangedValues.get(id);
+        if (lastValue == null || currentValue != lastValue) {
+            lastHasChangedValues.put(id, currentValue);
+            return true;
+        }
+
+        return false;
+    }
+
+    /**
+     * Runs action if any of the tunableNumbers have changed
+     *
+     * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
+     *        objects. Recommended approach is to pass the result of "hashCode()"
+     * @param action Callback to run when any of the tunable numbers have changed. Access tunable
+     *        numbers in order inputted in method
+     * @param tunableNumbers All tunable numbers to check
+     */
+    public static void ifChanged(int id, Consumer<double[]> action,
+        LoggedTunableNumber... tunableNumbers) {
+        if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+            action.accept(
+                Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
+        }
+    }
+
+    /** Runs action if any of the tunableNumbers have changed */
+    public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
+        ifChanged(id, values -> action.run(), tunableNumbers);
+    }
+
+    @Override
+    public double getAsDouble() {
+        return get();
+    }
+}

src/main/java/frc/robot/Constants.java

@@ -21,6 +21,7 @@
  * Constants file.
  */
 public final class Constants {
+    public static final boolean tuningMode = true;
     /**
      * Stick Deadband
      */

src/main/java/frc/robot/RobotContainer.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Robot.RobotRunType;
 import frc.robot.commands.TeleopSwerve;
+import frc.robot.subsystems.Elevator.Elevator;
 import frc.robot.subsystems.swerve.Swerve;
 import frc.robot.subsystems.swerve.SwerveIO;
 import frc.robot.subsystems.swerve.SwerveReal;
@@ -27,16 +28,19 @@ public class RobotContainer {
 
     /* Controllers */
     public final CommandXboxController driver = new CommandXboxController(Constants.DRIVER_ID);
+    public final CommandXboxController operator = new CommandXboxController(Constants.OPERATOR_ID);
 
     /* Subsystems */
     private Swerve s_Swerve;
+    private Elevator elevator;
 
     /**
      */
     public RobotContainer(RobotRunType runtimeType) {
         switch (runtimeType) {
             case kReal:
                 s_Swerve = new Swerve(new SwerveReal());
+                elevator = new Elevator();
                 break;
             case kSimulation:
                 s_Swerve = new Swerve(new SwerveSim());
@@ -51,6 +55,7 @@ public RobotContainer(RobotRunType runtimeType) {
 
         s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver,
             Constants.Swerve.isFieldRelative, Constants.Swerve.isOpenLoop));
+        elevator.setDefaultCommand(elevator.driveElevator(() -> operator.getLeftY()));
         // Configure the button bindings
         configureButtonBindings();
     }
@@ -66,6 +71,7 @@ private void configureButtonBindings() {
 
         /* Driver Buttons */
         driver.y().onTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset()));
+        operator.a().whileTrue(elevator.setPosition(0.5));
     }
 
     /**

src/main/java/frc/robot/subsystems/Elevator/Elevator.java

@@ -0,0 +1,92 @@
+package frc.robot.subsystems.Elevator;
+
+import java.util.function.DoubleSupplier;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.util.LoggedTunableNumber;
+
+public class Elevator extends SubsystemBase {
+
+
+    @SuppressWarnings("IOCheck")
+    private TalonFX motor = new TalonFX(27);
+
+    @SuppressWarnings("IOCheck")
+    private TalonFXConfiguration swerveAngleFXConfig = new TalonFXConfiguration();
+    /* angle motor control requests */
+
+    @SuppressWarnings("IOCheck")
+    private final PositionVoltage anglePosition = new PositionVoltage(0);
+
+    @SuppressWarnings("IOCheck")
+    private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0);
+
+    @SuppressWarnings("IOCheck")
+    DigitalInput bob = new DigitalInput(9);
+
+    boolean mode = true;
+
+    private LoggedTunableNumber kp = new LoggedTunableNumber("Elevator/kP", 0.0);
+    private LoggedTunableNumber ki = new LoggedTunableNumber("Elevator/kI", 0.0);
+    private LoggedTunableNumber kd = new LoggedTunableNumber("Elevator/kD", 0.0);
+    private LoggedTunableNumber ff = new LoggedTunableNumber("Elevator/ff", 0.0);
+
+    public Elevator() {
+        /* PID Config */
+        swerveAngleFXConfig.Slot0.kP = kp.getAsDouble();
+        swerveAngleFXConfig.Slot0.kI = ki.getAsDouble();
+        swerveAngleFXConfig.Slot0.kD = kd.getAsDouble();
+        swerveAngleFXConfig.Slot0.kG = ff.getAsDouble();
+        swerveAngleFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+        swerveAngleFXConfig.Feedback.SensorToMechanismRatio = 45.75;
+
+
+        motor.getConfigurator().apply(swerveAngleFXConfig);
+    }
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putBoolean("asdfads", bob.get());
+        if (bob.get()) {
+            motor.setPosition(0);
+        }
+        if (kp.hasChanged(kp.hashCode()) || ki.hasChanged(ki.hashCode())
+            || kd.hasChanged(kd.hashCode()) || ff.hasChanged(ff.hashCode())) {
+            swerveAngleFXConfig.Slot0.kP = kp.getAsDouble();
+            swerveAngleFXConfig.Slot0.kI = ki.getAsDouble();
+            swerveAngleFXConfig.Slot0.kD = kd.getAsDouble();
+            swerveAngleFXConfig.Slot0.kG = ff.getAsDouble();
+            motor.getConfigurator().apply(swerveAngleFXConfig);
+        }
+    }
+
+    public Command setPosition(double pos) {
+        return Commands.run(() -> motor.setControl(anglePosition.withPosition(pos)), this);
+    }
+
+    public Command driveElevator(DoubleSupplier power) {
+        return Commands.run(() -> {
+            if (mode) {
+                double p = MathUtil.applyDeadband(-power.getAsDouble(), .25);
+                if (bob.get() && p < 0) {
+                    p = 0;
+                }
+                motor.setControl(driveDutyCycle.withOutput(p));
+            } else {
+                motor.setControl(anglePosition.withSlot(0).withPosition(0));
+            }
+        }, this);
+    }
+
+
+
+}