-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
afe2d26
commit f3dc8ca
Showing
9 changed files
with
308 additions
and
9 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
164 changes: 164 additions & 0 deletions
164
src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
90 changes: 90 additions & 0 deletions
90
src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<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; | ||
| } | ||
| } |
32 changes: 32 additions & 0 deletions
32
src/main/java/org/team1540/robot2024/util/math/AverageFilter.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<Double> 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(); | ||
| } | ||
| } | ||
|
|
2 changes: 1 addition & 1 deletion
2
.../robot2024/util/PolynomialRegression.java → ...t2024/util/math/PolynomialRegression.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters