diff --git a/src/main/java/frc/lib/util/LoggedTunableNumber.java b/src/main/java/frc/lib/util/LoggedTunableNumber.java new file mode 100644 index 0000000..a1ca671 --- /dev/null +++ b/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 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 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(); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c5e04d3..52eb932 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 75fcf89..fe9f520 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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,9 +28,11 @@ 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; /** */ @@ -37,6 +40,7 @@ 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)); } /** diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java new file mode 100644 index 0000000..217af07 --- /dev/null +++ b/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); + } + + + +}