From 14bbadf56e397336bc289d1c254708d2d8c5ea53 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <114535782+mimizh2418@users.noreply.github.com> Date: Fri, 26 Jan 2024 18:34:44 -0800 Subject: [PATCH] Initial shooter code & basic shooter commands (#16) * feat: shooter IO interfaces + all hardware implementations * feat: shooter subsystem * feat: shooter sim IO implementations * feat: shooter commands * fix: clearer constants --- .../org/team1540/robot2024/Constants.java | 57 +++++- .../team1540/robot2024/RobotContainer.java | 33 ++-- .../commands/FeedForwardCharacterization.java | 2 +- .../subsystems/shooter/FlywheelsIO.java | 37 ++++ .../subsystems/shooter/FlywheelsIOSim.java | 74 ++++++++ .../shooter/FlywheelsIOTalonFX.java | 114 +++++++++++ .../robot2024/subsystems/shooter/Shooter.java | 178 ++++++++++++++++++ .../subsystems/shooter/ShooterPivotIO.java | 40 ++++ .../subsystems/shooter/ShooterPivotIOSim.java | 74 ++++++++ .../shooter/ShooterPivotIOTalonFX.java | 112 +++++++++++ .../robot2024/util/LoggedTunableNumber.java | 91 +++++++++ .../robot2024/util/math/AverageFilter.java | 32 ++++ .../util/{ => math}/PolynomialRegression.java | 2 +- 13 files changed, 832 insertions(+), 14 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java create mode 100644 src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java create mode 100644 src/main/java/org/team1540/robot2024/util/math/AverageFilter.java rename src/main/java/org/team1540/robot2024/util/{ => math}/PolynomialRegression.java (99%) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index a81310f2..23f20955 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -1,5 +1,6 @@ package org.team1540.robot2024; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; /** @@ -12,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 = true; // 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 { @@ -55,4 +57,57 @@ public static class Drivetrain { public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; } + + public static class Shooter { + public static class Flywheels { + // TODO: determine ids + public static final int LEFT_ID = 0; + public static final int RIGHT_ID = 0; + + public static final double GEAR_RATIO = 24.0 / 36.0; + public static final double SIM_MOI = 4.08232288e-4; + + // TODO: if it's tuned in simulation, it's tuned in real life + public static final double KP = 0.4; + public static final double KI = 0.0; + public static final double KD = 0.0; + public static final double KS = 0.01146; + public static final double KV = 0.07485; // TODO: this is what recalc says, may have to tune + + public static final double ERROR_TOLERANCE_RPM = 50; + } + + public static class Pivot { + // TODO: determine ids + public static final int MOTOR_ID = 0; + public static final int CANCODER_ID = 0; + + // TODO: figure this out + public static final double CANCODER_OFFSET_ROTS = 0; + // TODO: determine ratios + public static final double CANCODER_TO_PIVOT = 60.0 / 20.0; + public static final double MOTOR_TO_CANCODER = 33.0; + public static final double TOTAL_GEAR_RATIO = MOTOR_TO_CANCODER * CANCODER_TO_PIVOT; + public static final double SIM_LENGTH_METERS = Units.inchesToMeters(12.910); + // TODO: find the moi + public static final double SIM_MOI = 0.22552744227754662; + + public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(60.0); + public static final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(8.0); + + // TODO: tune pid + public static final double KP = 0.1; + public static final double KI = 0.0; + public static final double KD = 0.0; + public static final double KS = 0.0; + public static final double KG = 0.1; + public static final double KV = 0.1; + + public static final double CRUISE_VELOCITY_RPS = 4.0; + public static final double MAX_ACCEL_RPS2 = 40.0; + public static final double JERK_RPS3 = 2000; + + public static final Rotation2d ERROR_TOLERANCE = Rotation2d.fromDegrees(0.2); + } + } } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index d6e67634..bc466e76 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -9,10 +9,11 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; - +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.team1540.robot2024.commands.FeedForwardCharacterization; import org.team1540.robot2024.commands.SwerveDriveCommand; import org.team1540.robot2024.subsystems.drive.*; +import org.team1540.robot2024.subsystems.shooter.*; import org.team1540.robot2024.util.swerve.SwerveFactory; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -27,6 +28,7 @@ public class RobotContainer { // Subsystems public final Drivetrain drivetrain; + public final Shooter shooter; // Controller public final CommandXboxController driver = new CommandXboxController(0); @@ -35,6 +37,10 @@ public class RobotContainer { // Dashboard inputs public final LoggedDashboardChooser autoChooser; + // TODO: testing dashboard inputs, remove for comp + public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000); + public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000); + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -49,6 +55,7 @@ public RobotContainer() { new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)), new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)), new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT))); + shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX()); break; case SIM: @@ -60,22 +67,19 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); + shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim()); break; default: // Replayed robot, disable IO implementations drivetrain = new Drivetrain( - new GyroIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }); + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {}); break; } @@ -88,6 +92,10 @@ public RobotContainer() { "Drive FF Characterization", new FeedForwardCharacterization( drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity)); + autoChooser.addOption( + "Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60)); // Configure the button bindings configureButtonBindings(); @@ -108,6 +116,9 @@ private void configureButtonBindings() { drivetrain ).ignoringDisable(true) ); + + copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get())) + .onFalse(Commands.runOnce(shooter::stopFlywheels, shooter)); } /** 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 new file mode 100644 index 00000000..97b6cb0a --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java @@ -0,0 +1,37 @@ +package org.team1540.robot2024.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelsIO { + @AutoLog + class FlywheelsIOInputs { + public double leftAppliedVolts = 0.0; + public double leftCurrentAmps = 0.0; + public double leftVelocityRPM = 0.0; + + public double rightAppliedVolts = 0.0; + public double rightCurrentAmps = 0.0; + public double rightVelocityRPM = 0.0; + } + + /** + * Updates the set of loggable inputs + */ + default void updateInputs(FlywheelsIOInputs inputs) {} + + /** + * Runs open loop at the specified voltages + */ + default void setVoltage(double leftVolts, double rightVolts) {} + + + /** + * Runs closed loop at the specified RPMs + */ + default void setSpeeds(double leftRPM, double rightRPM) {} + + /** + * Configures the PID controller + */ + default void configPID(double kP, double kI, double kD) {} +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java new file mode 100644 index 00000000..947a6d92 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java @@ -0,0 +1,74 @@ +package org.team1540.robot2024.subsystems.shooter; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import org.team1540.robot2024.Constants; + +import static org.team1540.robot2024.Constants.Shooter.Flywheels.*; + +public class FlywheelsIOSim implements FlywheelsIO{ + private final FlywheelSim leftSim = + new FlywheelSim(DCMotor.getFalcon500(1), GEAR_RATIO, SIM_MOI); + private final FlywheelSim rightSim = + new FlywheelSim(DCMotor.getFalcon500(1), GEAR_RATIO, SIM_MOI); + + private final PIDController rightController = new PIDController(KP, KI, KD); + private final PIDController leftController = new PIDController(KP, KI, KD); + private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV); + + private boolean isClosedLoop; + private double leftSetpointRPS; + private double rightSetpointRPS; + + private double leftVolts = 0.0; + private double rightVolts = 0.0; + + @Override + public void updateInputs(FlywheelsIOInputs inputs) { + if (isClosedLoop) { + leftVolts = + leftController.calculate(leftSim.getAngularVelocityRPM() / 60, leftSetpointRPS) + + feedforward.calculate(leftSetpointRPS); + rightVolts = + rightController.calculate(rightSim.getAngularVelocityRPM() / 60, rightSetpointRPS) + + feedforward.calculate(rightSetpointRPS); + } + + leftSim.setInputVoltage(leftVolts); + rightSim.setInputVoltage(rightVolts); + leftSim.update(Constants.LOOP_PERIOD_SECS); + rightSim.update(Constants.LOOP_PERIOD_SECS); + + inputs.leftVelocityRPM = leftSim.getAngularVelocityRPM(); + inputs.leftAppliedVolts = leftVolts; + inputs.leftCurrentAmps = leftSim.getCurrentDrawAmps(); + + inputs.rightVelocityRPM = rightSim.getAngularVelocityRPM(); + inputs.rightAppliedVolts = rightVolts; + inputs.rightCurrentAmps = rightSim.getCurrentDrawAmps(); + } + + @Override + public void setVoltage(double leftVolts, double rightVolts) { + isClosedLoop = false; + this.leftVolts = leftVolts; + this.rightVolts = rightVolts; + } + + @Override + public void setSpeeds(double leftRPM, double rightRPM) { + isClosedLoop = true; + leftController.reset(); + rightController.reset(); + leftSetpointRPS = leftRPM / 60; + rightSetpointRPS = rightRPM / 60; + } + + @Override + public void configPID(double kP, double kI, double kD) { + leftController.setPID(kP, kI, kD); + rightController.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java new file mode 100644 index 00000000..0856d77c --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java @@ -0,0 +1,114 @@ +package org.team1540.robot2024.subsystems.shooter; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import static org.team1540.robot2024.Constants.Shooter.Flywheels.*; + +public class FlywheelsIOTalonFX implements FlywheelsIO { + private final TalonFX leftMotor = new TalonFX(LEFT_ID); + private final TalonFX rightMotor = new TalonFX(RIGHT_ID); + + private final StatusSignal leftVelocity = leftMotor.getVelocity(); + private final StatusSignal leftAppliedVolts = leftMotor.getMotorVoltage(); + private final StatusSignal leftCurrent = leftMotor.getSupplyCurrent(); + + private final StatusSignal rightVelocity = rightMotor.getVelocity(); + private final StatusSignal rightAppliedVolts = rightMotor.getMotorVoltage(); + private final StatusSignal rightCurrent = rightMotor.getSupplyCurrent(); + + private final VelocityVoltage leftVelocityCtrlReq = + new VelocityVoltage(0).withEnableFOC(false).withSlot(0); + private final VoltageOut leftVoltageCtrlReq = + new VoltageOut(0).withEnableFOC(false); + + private final VelocityVoltage rightVelocityCtrlReq = + new VelocityVoltage(0).withEnableFOC(false).withSlot(0); + private final VoltageOut rightVoltageCtrlReq = + new VoltageOut(0).withEnableFOC(false); + + public FlywheelsIOTalonFX() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + + // Shooter current limits are banned + config.CurrentLimits.SupplyCurrentLimitEnable = false; + config.CurrentLimits.StatorCurrentLimitEnable = false; + + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + config.Feedback.RotorToSensorRatio = 1.0; + + config.Slot0.kP = KP; + config.Slot0.kI = KI; + config.Slot0.kD = KD; + config.Slot0.kS = KS; + config.Slot0.kV = KV; + + leftMotor.getConfigurator().apply(config); + rightMotor.getConfigurator().apply(config); + leftMotor.setInverted(false); + rightMotor.setInverted(true); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50, + leftVelocity, + leftAppliedVolts, + leftCurrent, + rightVelocity, + rightAppliedVolts, + rightCurrent); + + leftMotor.optimizeBusUtilization(); + rightMotor.optimizeBusUtilization(); + } + + @Override + public void updateInputs(FlywheelsIOInputs inputs) { + BaseStatusSignal.refreshAll( + leftVelocity, + leftAppliedVolts, + leftCurrent, + rightVelocity, + rightAppliedVolts, + rightCurrent); + + inputs.leftVelocityRPM = leftVelocity.getValueAsDouble() * 60; + inputs.leftAppliedVolts = leftAppliedVolts.getValueAsDouble(); + inputs.leftCurrentAmps = leftCurrent.getValueAsDouble(); + + inputs.rightVelocityRPM = rightVelocity.getValueAsDouble() * 60; + inputs.rightAppliedVolts = rightAppliedVolts.getValueAsDouble(); + inputs.rightCurrentAmps = rightCurrent.getValueAsDouble(); + } + + @Override + public void setSpeeds(double leftRPM, double rightRPM) { + leftMotor.setControl(leftVelocityCtrlReq.withVelocity(leftRPM / 60)); + rightMotor.setControl(rightVelocityCtrlReq.withVelocity(rightRPM / 60)); + } + + @Override + public void setVoltage(double leftVolts, double rightVolts) { + leftMotor.setControl(leftVoltageCtrlReq.withOutput(leftVolts)); + rightMotor.setControl(rightVoltageCtrlReq.withOutput(rightVolts)); + } + + @Override + public void configPID(double kP, double kI, double kD) { + Slot0Configs pidConfigs = new Slot0Configs(); + leftMotor.getConfigurator().refresh(pidConfigs); + pidConfigs.kP = kP; + pidConfigs.kI = kI; + pidConfigs.kD = kD; + leftMotor.getConfigurator().apply(pidConfigs); + rightMotor.getConfigurator().apply(pidConfigs); + } +} 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..5639305c --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -0,0 +1,178 @@ +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.*; +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); // Units: RPM + private final AverageFilter rightSpeedFilter = new AverageFilter(20); // Units: RPM + 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", Flywheels.KP); + private final LoggedTunableNumber flywheelsKI = new LoggedTunableNumber("Shooter/Flywheels/kI", Flywheels.KI); + private final LoggedTunableNumber flywheelsKD = new LoggedTunableNumber("Shooter/Flywheels/kD", Flywheels.KD); + + private final LoggedTunableNumber pivotKP = new LoggedTunableNumber("Shooter/Pivot/kP", Pivot.KP); + private final LoggedTunableNumber pivotKI = new LoggedTunableNumber("Shooter/Pivot/kI", Pivot.KI); + private final LoggedTunableNumber pivotKD = new LoggedTunableNumber("Shooter/Pivot/kD", Pivot.KD); + + public Shooter(ShooterPivotIO pivotIO, FlywheelsIO flywheelsIO) { + this.pivotIO = pivotIO; + this.flywheelsIO = flywheelsIO; + } + + @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); + } + + public void setPivotBrakeMode(boolean isBrakeMode) { + pivotIO.setBrakeMode(isBrakeMode); + } + + /** + * 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(), Flywheels.ERROR_TOLERANCE_RPM) && + MathUtil.isNear(rightFlywheelSetpointRPM, rightSpeedFilter.getAverage(), Flywheels.ERROR_TOLERANCE_RPM); + } + + /** + * Gets whether the pivot is at its setpoint + */ + public boolean isPivotAtSetpoint() { + return MathUtil.isNear( + pivotSetpoint.getRotations(), pivotPositionFilter.getAverage(), Pivot.ERROR_TOLERANCE.getRotations()); + } + + public Command spinUpCommand(double leftSetpoint, double rightSetpoint) { + return new FunctionalCommand( + () -> setFlywheelSpeeds(leftSetpoint, rightSetpoint), + () -> {}, + (ignored) -> {}, + this::areFlywheelsSpunUp, + this); + } + + public Command setPivotPositionCommand(Rotation2d setpoint) { + return new FunctionalCommand( + () -> setPivotPosition(setpoint), + () -> {}, + (ignored) -> {}, + this::isPivotAtSetpoint, + this); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java new file mode 100644 index 00000000..0395990b --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java @@ -0,0 +1,40 @@ +package org.team1540.robot2024.subsystems.shooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ShooterPivotIO { + @AutoLog + class ShooterPivotIOInputs { + public Rotation2d position = new Rotation2d(); + public Rotation2d absolutePosition = new Rotation2d(); + public double velocityRPS = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + } + + /** + * Updates the set of loggable inputs + */ + default void updateInputs(ShooterPivotIOInputs inputs) {} + + /** + * Runs closed loop to the specified position + */ + default void setPosition(Rotation2d position) {} + + /** + * Runs open loop at the specified voltage + */ + default void setVoltage(double volts) {} + + /** + * Sets the neutral output mode + */ + default void setBrakeMode(boolean isBrakeMode) {} + + /** + * Configures the PID controller + */ + default void configPID(double kP, double kI, double kD) {} +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java new file mode 100644 index 00000000..7c8d0cb4 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java @@ -0,0 +1,74 @@ +package org.team1540.robot2024.subsystems.shooter; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import org.team1540.robot2024.Constants; + +import static org.team1540.robot2024.Constants.Shooter.Pivot.*; + +public class ShooterPivotIOSim implements ShooterPivotIO { + private final SingleJointedArmSim sim = + new SingleJointedArmSim( + DCMotor.getFalcon500Foc(1), + TOTAL_GEAR_RATIO, + SIM_MOI, + SIM_LENGTH_METERS, + MIN_ANGLE.getRadians(), + MAX_ANGLE.getRadians(), + true, + MIN_ANGLE.getRadians()); + + private final ProfiledPIDController controller = + new ProfiledPIDController(KP, KI, KD, new TrapezoidProfile.Constraints(CRUISE_VELOCITY_RPS, MAX_ACCEL_RPS2)); + private final ArmFeedforward feedforward = new ArmFeedforward(KS, KG, KV); + + private boolean isClosedLoop; + private TrapezoidProfile.State goalState; + + private double appliedVolts; + + @Override + public void updateInputs(ShooterPivotIOInputs inputs) { + if (isClosedLoop) { + appliedVolts = + controller.calculate(Units.radiansToRotations(sim.getAngleRads()), goalState) + + feedforward.calculate( + Units.rotationsToRadians(controller.getSetpoint().position), + controller.getSetpoint().velocity); + } + + sim.setInputVoltage(appliedVolts); + sim.update(Constants.LOOP_PERIOD_SECS); + inputs.position = Rotation2d.fromRadians(sim.getAngleRads()); + inputs.absolutePosition = Rotation2d.fromRadians(sim.getAngleRads()); + inputs.velocityRPS = Units.radiansToRotations(sim.getVelocityRadPerSec()); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void setPosition(Rotation2d position) { + controller.reset( + Units.radiansToRotations(sim.getAngleRads()), + Units.radiansToRotations(sim.getVelocityRadPerSec()) + ); + isClosedLoop = true; + goalState = new TrapezoidProfile.State(position.getRadians(), 0); + } + + @Override + public void setVoltage(double volts) { + isClosedLoop = false; + appliedVolts = volts; + } + + @Override + public void configPID(double kP, double kI, double kD) { + controller.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java new file mode 100644 index 00000000..73794b20 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java @@ -0,0 +1,112 @@ +package org.team1540.robot2024.subsystems.shooter; + +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; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.*; +import edu.wpi.first.math.geometry.Rotation2d; + +import static org.team1540.robot2024.Constants.Shooter.Pivot.*; + +public class ShooterPivotIOTalonFX implements ShooterPivotIO { + private final TalonFX motor = new TalonFX(MOTOR_ID); + private final CANcoder cancoder = new CANcoder(CANCODER_ID); + + private final StatusSignal position = motor.getPosition(); + private final StatusSignal absolutePosition = cancoder.getAbsolutePosition(); + private final StatusSignal velocity = motor.getVelocity(); + private final StatusSignal appliedVoltage = motor.getMotorVoltage(); + private final StatusSignal current = motor.getSupplyCurrent(); + + private final MotionMagicVoltage positionCtrlReq = new MotionMagicVoltage(0).withSlot(0); + private final VoltageOut voltageCtrlReq = new VoltageOut(0); + + public ShooterPivotIOTalonFX() { + TalonFXConfiguration motorConfig = new TalonFXConfiguration(); + + // TODO: find invert + motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + motorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + motorConfig.Feedback.FeedbackRemoteSensorID = CANCODER_ID; + 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; + motorConfig.Slot0.kS = KS; + motorConfig.Slot0.kG = KG; + motorConfig.Slot0.kV = KV; + + motorConfig.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY_RPS; + motorConfig.MotionMagic.MotionMagicAcceleration = MAX_ACCEL_RPS2; + motorConfig.MotionMagic.MotionMagicJerk = JERK_RPS3; + + CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); + cancoderConfig.MagnetSensor.MagnetOffset = CANCODER_OFFSET_ROTS; + // TODO: find invert + cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + cancoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; + + cancoder.getConfigurator().apply(cancoderConfig); + motor.getConfigurator().apply(motorConfig); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50, + position, + absolutePosition, + velocity, + appliedVoltage, + current); + + motor.optimizeBusUtilization(); + cancoder.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ShooterPivotIOInputs inputs) { + inputs.position = Rotation2d.fromRotations(position.getValueAsDouble()); + inputs.absolutePosition = Rotation2d.fromRotations(absolutePosition.getValueAsDouble() / CANCODER_TO_PIVOT); + inputs.velocityRPS = velocity.getValueAsDouble(); + inputs.appliedVolts = appliedVoltage.getValueAsDouble(); + inputs.currentAmps = current.getValueAsDouble(); + } + + @Override + public void setPosition(Rotation2d position) { + motor.setControl(positionCtrlReq.withPosition(position.getRotations())); + } + + @Override + public void setVoltage(double volts) { + motor.setControl(voltageCtrlReq.withOutput(volts)); + } + + @Override + public void setBrakeMode(boolean isBrakeMode) { + motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + @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..e9fc1aa7 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java @@ -0,0 +1,91 @@ +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) { + if (!Constants.tuningMode) return false; + 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;