diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 18981e09..2bc223c9 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -13,9 +13,10 @@ */ public final class Constants { public static final boolean IS_COMPETITION_ROBOT = true; + // Whether to pull PID constants from SmartDashboard + public static final boolean tuningMode = false; // TODO: DO NOT SET TO TRUE FOR COMP private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY - public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; public enum Mode { diff --git a/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java b/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java index 1b5903eb..fc07d569 100644 --- a/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java +++ b/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; -import org.team1540.robot2024.util.PolynomialRegression; +import org.team1540.robot2024.util.math.PolynomialRegression; import java.util.LinkedList; import java.util.List; diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java index 63a5c02b..97b6cb0a 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java @@ -8,12 +8,10 @@ class FlywheelsIOInputs { public double leftAppliedVolts = 0.0; public double leftCurrentAmps = 0.0; public double leftVelocityRPM = 0.0; - public double leftSetpointRPM = 0.0; public double rightAppliedVolts = 0.0; public double rightCurrentAmps = 0.0; public double rightVelocityRPM = 0.0; - public double rightSetpointRPM = 0.0; } /** @@ -30,7 +28,7 @@ default void setVoltage(double leftVolts, double rightVolts) {} /** * Runs closed loop at the specified RPMs */ - default void setVelocity(double leftRPM, double rightRPM) {} + default void setSpeeds(double leftRPM, double rightRPM) {} /** * Configures the PID controller diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java index f506ed52..0856d77c 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java @@ -81,18 +81,16 @@ public void updateInputs(FlywheelsIOInputs inputs) { rightCurrent); inputs.leftVelocityRPM = leftVelocity.getValueAsDouble() * 60; - inputs.leftSetpointRPM = leftVelocityCtrlReq.Velocity * 60; inputs.leftAppliedVolts = leftAppliedVolts.getValueAsDouble(); inputs.leftCurrentAmps = leftCurrent.getValueAsDouble(); inputs.rightVelocityRPM = rightVelocity.getValueAsDouble() * 60; - inputs.rightSetpointRPM = rightVelocityCtrlReq.Velocity * 60; inputs.rightAppliedVolts = rightAppliedVolts.getValueAsDouble(); inputs.rightCurrentAmps = rightCurrent.getValueAsDouble(); } @Override - public void setVelocity(double leftRPM, double rightRPM) { + public void setSpeeds(double leftRPM, double rightRPM) { leftMotor.setControl(leftVelocityCtrlReq.withVelocity(leftRPM / 60)); rightMotor.setControl(rightVelocityCtrlReq.withVelocity(rightRPM / 60)); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java new file mode 100644 index 00000000..d84f0cd3 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -0,0 +1,164 @@ +package org.team1540.robot2024.subsystems.shooter; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; +import org.team1540.robot2024.util.LoggedTunableNumber; +import org.team1540.robot2024.util.math.AverageFilter; + +import static org.team1540.robot2024.Constants.Shooter.*; + +public class Shooter extends SubsystemBase { + private final ShooterPivotIO pivotIO; + private final ShooterPivotIOInputsAutoLogged pivotInputs = new ShooterPivotIOInputsAutoLogged(); + + private final FlywheelsIO flywheelsIO; + private final FlywheelsIOInputsAutoLogged flywheelInputs = new FlywheelsIOInputsAutoLogged(); + + private final AverageFilter leftSpeedFilter = new AverageFilter(20); + private final AverageFilter rightSpeedFilter = new AverageFilter(20); + private final AverageFilter pivotPositionFilter = new AverageFilter(10); // Units: rotations + + private double leftFlywheelSetpointRPM; + private double rightFlywheelSetpointRPM; + private Rotation2d pivotSetpoint; + + private final LoggedTunableNumber flywheelsKP = new LoggedTunableNumber("Shooter/Flywheels/kP"); + private final LoggedTunableNumber flywheelsKI = new LoggedTunableNumber("Shooter/Flywheels/kI"); + private final LoggedTunableNumber flywheelsKD = new LoggedTunableNumber("Shooter/Flywheels/kD"); + + private final LoggedTunableNumber pivotKP = new LoggedTunableNumber("Shooter/Pivot/kP"); + private final LoggedTunableNumber pivotKI = new LoggedTunableNumber("Shooter/Pivot/kI"); + private final LoggedTunableNumber pivotKD = new LoggedTunableNumber("Shooter/Pivot/kD"); + + public Shooter(ShooterPivotIO pivotIO, FlywheelsIO flywheelsIO) { + this.pivotIO = pivotIO; + this.flywheelsIO = flywheelsIO; + + // Init tunable numbers + flywheelsKP.initDefault(Flywheels.KP); + flywheelsKI.initDefault(Flywheels.KI); + flywheelsKD.initDefault(Flywheels.KD); + + pivotKP.initDefault(Pivot.KP); + pivotKI.initDefault(Pivot.KI); + pivotKD.initDefault(Pivot.KD); + } + + @Override + public void periodic() { + // Update & process inputs + pivotIO.updateInputs(pivotInputs); + flywheelsIO.updateInputs(flywheelInputs); + Logger.processInputs("Shooter/Pivot", pivotInputs); + Logger.processInputs("Shooter/Flywheels", flywheelInputs); + + // Update tunable numbers + if (flywheelsKP.hasChanged(hashCode()) || flywheelsKI.hasChanged(hashCode()) || flywheelsKD.hasChanged(hashCode())) { + flywheelsIO.configPID(flywheelsKP.get(), flywheelsKI.get(), flywheelsKD.get()); + } + if (pivotKP.hasChanged(hashCode()) || pivotKI.hasChanged(hashCode()) || pivotKD.hasChanged(hashCode())) { + pivotIO.configPID(pivotKP.get(), pivotKI.get(), pivotKD.get()); + } + + // Add values to filters + leftSpeedFilter.add(getLeftFlywheelSpeed()); + rightSpeedFilter.add(getRightFlywheelSpeed()); + pivotPositionFilter.add(getPivotPosition().getRotations()); + } + + /** + * Sets the left and right speeds of the flywheels + */ + public void setFlywheelSpeeds(double leftSpeedRPM, double rightSpeedRPM) { + leftFlywheelSetpointRPM = leftSpeedRPM; + rightFlywheelSetpointRPM = rightSpeedRPM; + leftSpeedFilter.clear(); + rightSpeedFilter.clear(); + flywheelsIO.setSpeeds(leftSpeedRPM, rightSpeedRPM); + } + + /** + * Sets the voltages to each side of the flywheel + */ + public void setFlywheelVolts(double rightVolts, double leftVolts) { + flywheelsIO.setVoltage( + MathUtil.clamp(rightVolts, -12, 12), + MathUtil.clamp(leftVolts, -12, 12) + ); + } + + /** + * Applies neutral output to the flywheels + */ + public void stopFlywheels() { + setFlywheelVolts(0, 0); + } + + /** + * Sets the position of the pivot, using parallel to the floor as 0 + */ + public void setPivotPosition(Rotation2d position) { + pivotSetpoint = Rotation2d.fromRotations( + MathUtil.clamp( + position.getRotations(), + Pivot.MIN_ANGLE.getRotations(), + Pivot.MAX_ANGLE.getRotations() + ) + ); + pivotPositionFilter.clear(); + pivotIO.setPosition(pivotSetpoint); + } + + /** + * Sets the voltage to the pivot + */ + public void setPivotVolts(double volts) { + pivotIO.setVoltage(MathUtil.clamp(volts, -12, 12)); + } + + /** + * Applies neutral output to the pivot + */ + public void stopPivot() { + setPivotVolts(0); + } + + /** + * Gets the speed of the left flywheel in RPM + */ + public double getLeftFlywheelSpeed() { + return flywheelInputs.leftVelocityRPM; + } + + /** + * Gets the speed of the right flywheel in RPM + */ + public double getRightFlywheelSpeed() { + return flywheelInputs.rightVelocityRPM; + } + + /** + * Gets the position of the pivot + */ + public Rotation2d getPivotPosition() { + return pivotInputs.position; + } + + /** + * Gets whether the flywheels are spun up to their setpoints + */ + public boolean areFlywheelsSpunUp() { + return + MathUtil.isNear(leftFlywheelSetpointRPM, leftSpeedFilter.getAverage(), 50) && + MathUtil.isNear(rightFlywheelSetpointRPM, rightSpeedFilter.getAverage(), 50); + } + + /** + * Gets whether the pivot is at its setpoint + */ + public boolean isPivotAtSetpoint() { + return MathUtil.isNear(pivotSetpoint.getRotations(), pivotPositionFilter.getAverage(), 0.2/360); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java index 9e73e7e8..9d6d935a 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; @@ -38,6 +39,11 @@ public ShooterPivotIOTalonFX() { motorConfig.Feedback.SensorToMechanismRatio = CANCODER_TO_PIVOT; motorConfig.Feedback.RotorToSensorRatio = MOTOR_TO_CANCODER; + motorConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + motorConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.getRotations(); + motorConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + motorConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.getRotations(); + motorConfig.Slot0.kP = KP; motorConfig.Slot0.kI = KI; motorConfig.Slot0.kD = KD; @@ -89,4 +95,14 @@ public void setPosition(Rotation2d position) { public void setVoltage(double volts) { motor.setControl(voltageCtrlReq.withOutput(volts)); } + + @Override + public void configPID(double kP, double kI, double kD) { + Slot0Configs pidConfigs = new Slot0Configs(); + motor.getConfigurator().refresh(pidConfigs); + pidConfigs.kP = kP; + pidConfigs.kI = kI; + pidConfigs.kD = kD; + motor.getConfigurator().apply(pidConfigs); + } } diff --git a/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java b/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java new file mode 100644 index 00000000..f51ae9d6 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java @@ -0,0 +1,90 @@ +package org.team1540.robot2024.util; + +import java.util.HashMap; +import java.util.Map; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import org.team1540.robot2024.Constants; + +// NOTE: This file is available at +// https://github.com/Mechanical-Advantage/RobotCode2023/blob/main/src/main/java/org/littletonrobotics/frc2023/util/LoggedTunableNumber.java + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber { + 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; + } +} diff --git a/src/main/java/org/team1540/robot2024/util/math/AverageFilter.java b/src/main/java/org/team1540/robot2024/util/math/AverageFilter.java new file mode 100644 index 00000000..fd47b0d0 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/util/math/AverageFilter.java @@ -0,0 +1,32 @@ +package org.team1540.robot2024.util.math; + +import java.util.LinkedList; + +// hopefully we won't have skill issues with this class anymore (2023orwil moment) +public class AverageFilter { + private final LinkedList items = new LinkedList<>(); + private double sum = 0; + private final int size; + + public AverageFilter(int size) { + this.size = size; + } + + public void clear() { + items.clear(); + sum = 0; + } + + public void add(double value) { + items.addLast(value); + sum += value; + if (items.size() > this.size) { + sum -= items.removeFirst(); + } + } + + public double getAverage() { + return sum/(double) items.size(); + } +} + diff --git a/src/main/java/org/team1540/robot2024/util/PolynomialRegression.java b/src/main/java/org/team1540/robot2024/util/math/PolynomialRegression.java similarity index 99% rename from src/main/java/org/team1540/robot2024/util/PolynomialRegression.java rename to src/main/java/org/team1540/robot2024/util/math/PolynomialRegression.java index 1d7a2479..ecf50fd7 100644 --- a/src/main/java/org/team1540/robot2024/util/PolynomialRegression.java +++ b/src/main/java/org/team1540/robot2024/util/math/PolynomialRegression.java @@ -1,4 +1,4 @@ -package org.team1540.robot2024.util; +package org.team1540.robot2024.util.math; import Jama.Matrix; import Jama.QRDecomposition;