From 68f158c7fce17540e63e903c370a5b8f861095cc Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Thu, 30 Nov 2023 22:28:18 -0800 Subject: [PATCH 1/9] feat: shooter code, tuned-ish pid --- .../team1540/bunnybotTank2023/Constants.java | 48 +++++++++++++++ .../org/team1540/bunnybotTank2023/Robot.java | 1 + .../bunnybotTank2023/RobotContainer.java | 12 +++- .../commands/shooter/Shooter.java | 58 +++++++++++++++++++ .../shooter/ShooterSpinUpCommand.java | 23 ++++++++ .../io/shooter/ShooterIO.java | 20 +++++++ .../io/shooter/ShooterIOReal.java | 54 +++++++++++++++++ .../io/shooter/ShooterIOSim.java | 53 +++++++++++++++++ .../utils/LoggedTunableNumber.java | 21 +++++++ 9 files changed, 289 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIO.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOSim.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/utils/LoggedTunableNumber.java diff --git a/src/main/java/org/team1540/bunnybotTank2023/Constants.java b/src/main/java/org/team1540/bunnybotTank2023/Constants.java index 5906abf..7645d25 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/Constants.java +++ b/src/main/java/org/team1540/bunnybotTank2023/Constants.java @@ -1,8 +1,17 @@ package org.team1540.bunnybotTank2023; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; public final class Constants { + // Allow PID constants to be tuned from a dashboard input or not + public static final boolean TUNING_MODE = false; + // Simulation mode, irrelevant for code running on physical robot public static final SimulationMode simulationMode = SimulationMode.SIM; @@ -42,4 +51,43 @@ public static class DrivetrainConstants { public static final int BACK_LEFT_ID = 3; public static final int BACK_RIGHT_ID = 4; } + + public static class ShooterConstants { + public static final int LEADER_ID = 20; + public static final int FOLLOWER_ID = 21; + + public static final double KP = 0.7; // TODO: 11/30/2023 tuned, doesn't work well for values < 1500 rpm + public static final double KI = 0.0; + public static final double KD = 0; + public static final double KS = 0.19; + public static final double KV = 0.1149; + + public static final double ERROR_TOLERANCE_RPM = 30; // TODO: 11/28/2023 tune + + public static final double MOI = 0.0014924622; + + public static final TalonFXConfiguration MOTOR_CONFIG = new TalonFXConfiguration(); + static { + MotorOutputConfigs outputConfigs = new MotorOutputConfigs(); + outputConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + outputConfigs.NeutralMode = NeutralModeValue.Coast; + + CurrentLimitsConfigs currentLimitConfigs = new CurrentLimitsConfigs(); + currentLimitConfigs.SupplyCurrentLimitEnable = true; + currentLimitConfigs.SupplyCurrentLimit = 40; + currentLimitConfigs.SupplyCurrentThreshold = 60; + currentLimitConfigs.SupplyTimeThreshold = 0.1; + + Slot0Configs pidConfigs = new Slot0Configs(); + pidConfigs.kP = KP; + pidConfigs.kI = KI; + pidConfigs.kD = KD; + pidConfigs.kS = KS; + pidConfigs.kV = KV; + + MOTOR_CONFIG.MotorOutput = outputConfigs; + MOTOR_CONFIG.CurrentLimits = currentLimitConfigs; + MOTOR_CONFIG.Slot0 = pidConfigs; + } + } } diff --git a/src/main/java/org/team1540/bunnybotTank2023/Robot.java b/src/main/java/org/team1540/bunnybotTank2023/Robot.java index 2b4f0cf..2409d1c 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/Robot.java +++ b/src/main/java/org/team1540/bunnybotTank2023/Robot.java @@ -98,6 +98,7 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } + robotContainer.shooter.stop(); } @Override diff --git a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java index 5ff015c..72cb863 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java +++ b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java @@ -6,12 +6,17 @@ package org.team1540.bunnybotTank2023; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.team1540.bunnybotTank2023.commands.auto.AutoShoot5RamTotes; import org.team1540.bunnybotTank2023.commands.drivetrain.Drivetrain; import org.team1540.bunnybotTank2023.commands.drivetrain.TankdriveCommand; +import org.team1540.bunnybotTank2023.commands.shooter.Shooter; import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOSim; import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOReal; +import org.team1540.bunnybotTank2023.io.shooter.ShooterIOReal; +import org.team1540.bunnybotTank2023.io.shooter.ShooterIOSim; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -22,18 +27,23 @@ public class RobotContainer { // Subsystems Drivetrain drivetrain; + Shooter shooter; // Controllers CommandXboxController driver = new CommandXboxController(0); CommandXboxController copilot = new CommandXboxController(1); + LoggedDashboardNumber shooterRPM = new LoggedDashboardNumber("shooterRPM", 0); + public RobotContainer() { if (Robot.isReal()) { // Initialize subsystems with hardware IO drivetrain = new Drivetrain(new DrivetrainIOReal()); + shooter = new Shooter(new ShooterIOReal()); } else { // Initialize subsystems with simulation IO drivetrain = new Drivetrain(new DrivetrainIOSim()); + shooter = new Shooter(new ShooterIOSim()); } setDefaultCommands(); configureButtonBindings(); @@ -42,7 +52,7 @@ public RobotContainer() { /** Use this method to define your trigger->command mappings. */ private void configureButtonBindings() { - + driver.a().onTrue(new InstantCommand(() -> shooter.setVelocity(shooterRPM.get()))).onFalse(new InstantCommand(() -> shooter.stop())); } private void setDefaultCommands() { diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java index 0e9d947..6d32c77 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java @@ -1,6 +1,64 @@ package org.team1540.bunnybotTank2023.commands.shooter; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; +import org.team1540.bunnybotTank2023.io.shooter.ShooterIO; +import org.team1540.bunnybotTank2023.io.shooter.ShooterInputsAutoLogged; +import org.team1540.bunnybotTank2023.utils.AverageFilter; +import org.team1540.bunnybotTank2023.utils.LoggedTunableNumber; + +import static org.team1540.bunnybotTank2023.Constants.*; public class Shooter extends SubsystemBase { + + private final LoggedTunableNumber kP = new LoggedTunableNumber("Shooter/kP", ShooterConstants.KP); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Shooter/kI", ShooterConstants.KI); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Shooter/kD", ShooterConstants.KD); + + private final ShooterIO io; + private final ShooterInputsAutoLogged inputs = new ShooterInputsAutoLogged(); + + private final AverageFilter averageFilter = new AverageFilter(20); // TODO: 11/28/2023 tune filter size + + private double setpoint = 0; + private boolean isClosedLoop = false; + + public Shooter(ShooterIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.getInstance().processInputs("Shooter", inputs); + + if (TUNING_MODE && (kP.hasChanged() || kI.hasChanged() || kD.hasChanged())) { + io.configurePID(kP.get(), kI.get(), kD.get()); + } + + averageFilter.add(inputs.velocityRPM); + Logger.getInstance().recordOutput("Shooter/SetpointRPM", setpoint); + Logger.getInstance().recordOutput("Shooter/FilteredVelocity", averageFilter.getAverage()); + } + + public void setVelocity(double speedRPM) { + setpoint = speedRPM; + isClosedLoop = true; + averageFilter.clear(); + io.setVelocity(speedRPM); + } + + public void setVoltage(double volts) { + isClosedLoop = false; + io.setVoltage(volts); + } + + public void stop() { + setVoltage(0); + } + + public boolean isAtSetpoint() { + if (!isClosedLoop) return false; + return Math.abs(averageFilter.getAverage() - setpoint) < ShooterConstants.ERROR_TOLERANCE_RPM; + } } diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java new file mode 100644 index 0000000..46addf1 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java @@ -0,0 +1,23 @@ +package org.team1540.bunnybotTank2023.commands.shooter; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterSpinUpCommand extends CommandBase { + private final Shooter shooter; + private final double setpoint; + + public ShooterSpinUpCommand(Shooter shooter, double rpm) { + this.shooter = shooter; + this.setpoint = rpm; + } + + @Override + public void initialize() { + shooter.setVelocity(setpoint); + } + + @Override + public boolean isFinished() { + return shooter.isAtSetpoint(); + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIO.java b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIO.java new file mode 100644 index 0000000..7d34803 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIO.java @@ -0,0 +1,20 @@ +package org.team1540.bunnybotTank2023.io.shooter; + +import org.littletonrobotics.junction.AutoLog; + +public interface ShooterIO { + @AutoLog + class ShooterInputs { + public double velocityRPM = 0; + public double appliedVolts = 0; + public double currentAmps = 0; + } + + void setVelocity(double speedRPM); + + void setVoltage(double volts); + + void updateInputs(ShooterInputs inputs); + + void configurePID(double kP, double kI, double kD); +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java new file mode 100644 index 0000000..1b2cd97 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java @@ -0,0 +1,54 @@ +package org.team1540.bunnybotTank2023.io.shooter; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; + +import static org.team1540.bunnybotTank2023.Constants.*; + +public class ShooterIOReal implements ShooterIO{ + private final TalonFX leftMotor = new TalonFX(ShooterConstants.LEADER_ID); + private final TalonFX rightMotor = new TalonFX(ShooterConstants.FOLLOWER_ID); + + private final StatusSignal velocity = leftMotor.getVelocity(); + private final StatusSignal busVoltage = leftMotor.getSupplyVoltage(); + private final StatusSignal dutyCycle = leftMotor.getDutyCycle(); + private final StatusSignal current = leftMotor.getSupplyCurrent(); + + public ShooterIOReal() { + leftMotor.getConfigurator().apply(ShooterConstants.MOTOR_CONFIG); + rightMotor.getConfigurator().apply(ShooterConstants.MOTOR_CONFIG); + rightMotor.setControl(new Follower(ShooterConstants.LEADER_ID, true)); + } + + @Override + public void updateInputs(ShooterInputs inputs) { + inputs.velocityRPM = velocity.refresh().getValue() * 60; + inputs.appliedVolts = busVoltage.refresh().getValue() * dutyCycle.refresh().getValue(); + inputs.currentAmps = current.refresh().getValue(); + } + + @Override + public void setVoltage(double volts) { + leftMotor.setControl(new VoltageOut(volts)); + } + + @Override + public void setVelocity(double speedRPM) { + leftMotor.setControl(new VelocityVoltage(speedRPM / 60)); + System.out.println("setting rpm"); + } + + @Override + public void configurePID(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); + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOSim.java b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOSim.java new file mode 100644 index 0000000..d7b301b --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOSim.java @@ -0,0 +1,53 @@ +package org.team1540.bunnybotTank2023.io.shooter; + +import edu.wpi.first.math.MathUtil; +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 static org.team1540.bunnybotTank2023.Constants.*; + +public class ShooterIOSim implements ShooterIO { + private final FlywheelSim sim = new FlywheelSim(DCMotor.getFalcon500(2), 1, ShooterConstants.MOI); + + private final PIDController pid = new PIDController(ShooterConstants.KP, ShooterConstants.KI, ShooterConstants.KD); + private final SimpleMotorFeedforward feedforward = + new SimpleMotorFeedforward(ShooterConstants.KS, ShooterConstants.KV); + + private double appliedVolts = 0; + private boolean isClosedLoop = false; + private double closedLoopSetpoint = 0; + + @Override + public void updateInputs(ShooterInputs inputs) { + if (isClosedLoop) { + appliedVolts = + pid.calculate(sim.getAngularVelocityRPM() * 60, closedLoopSetpoint) + + feedforward.calculate(closedLoopSetpoint); + setVoltage(appliedVolts); + } + + inputs.velocityRPM = sim.getAngularVelocityRPM(); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void setVoltage(double volts) { + isClosedLoop = false; + appliedVolts = MathUtil.clamp(volts, -12, 12); + sim.setInputVoltage(appliedVolts); + } + + @Override + public void setVelocity(double speedRPM) { + isClosedLoop = true; + closedLoopSetpoint = speedRPM / 60; + } + + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/utils/LoggedTunableNumber.java b/src/main/java/org/team1540/bunnybotTank2023/utils/LoggedTunableNumber.java new file mode 100644 index 0000000..fbaf9a2 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/utils/LoggedTunableNumber.java @@ -0,0 +1,21 @@ +package org.team1540.bunnybotTank2023.utils; + +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +public class LoggedTunableNumber extends LoggedDashboardNumber { + private double lastHasChangedValue; + + public LoggedTunableNumber(String key, double defaultValue) { + super(key, defaultValue); + lastHasChangedValue = defaultValue; + } + + public boolean hasChanged() { + double currentValue = get(); + if (currentValue != lastHasChangedValue) { + lastHasChangedValue = currentValue; + return true; + } + return false; + } +} From c6abc87565252e36af7d52198877054a553ecc8e Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 1 Dec 2023 18:45:36 -0800 Subject: [PATCH 2/9] feat: tuned shooter pid --- .../java/org/team1540/bunnybotTank2023/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/team1540/bunnybotTank2023/Constants.java b/src/main/java/org/team1540/bunnybotTank2023/Constants.java index 7645d25..21fe79e 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/Constants.java +++ b/src/main/java/org/team1540/bunnybotTank2023/Constants.java @@ -10,7 +10,7 @@ public final class Constants { // Allow PID constants to be tuned from a dashboard input or not - public static final boolean TUNING_MODE = false; + public static final boolean TUNING_MODE = true; // Simulation mode, irrelevant for code running on physical robot public static final SimulationMode simulationMode = SimulationMode.SIM; @@ -56,13 +56,13 @@ public static class ShooterConstants { public static final int LEADER_ID = 20; public static final int FOLLOWER_ID = 21; - public static final double KP = 0.7; // TODO: 11/30/2023 tuned, doesn't work well for values < 1500 rpm - public static final double KI = 0.0; + public static final double KP = 0.6; // TODO: 11/30/2023 tuned, doesn't work well for values < 1000 rpm + public static final double KI = 0.1; public static final double KD = 0; public static final double KS = 0.19; public static final double KV = 0.1149; - public static final double ERROR_TOLERANCE_RPM = 30; // TODO: 11/28/2023 tune + public static final double ERROR_TOLERANCE_RPM = 30; public static final double MOI = 0.0014924622; From 3e8898ae04dff7fa31c328fd77c1638e4491a666 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 1 Dec 2023 21:06:04 -0800 Subject: [PATCH 3/9] fix: shooter simulation now works --- .../java/org/team1540/bunnybotTank2023/Constants.java | 2 +- .../commands/auto/AutoShoot5RamTotes.java | 1 - .../bunnybotTank2023/io/shooter/ShooterIOSim.java | 8 +++++--- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/team1540/bunnybotTank2023/Constants.java b/src/main/java/org/team1540/bunnybotTank2023/Constants.java index 21fe79e..ed576bf 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/Constants.java +++ b/src/main/java/org/team1540/bunnybotTank2023/Constants.java @@ -57,7 +57,7 @@ public static class ShooterConstants { public static final int FOLLOWER_ID = 21; public static final double KP = 0.6; // TODO: 11/30/2023 tuned, doesn't work well for values < 1000 rpm - public static final double KI = 0.1; + public static final double KI = 0.07; public static final double KD = 0; public static final double KS = 0.19; public static final double KV = 0.1149; diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/auto/AutoShoot5RamTotes.java b/src/main/java/org/team1540/bunnybotTank2023/commands/auto/AutoShoot5RamTotes.java index 9e6fcf3..c260531 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/auto/AutoShoot5RamTotes.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/auto/AutoShoot5RamTotes.java @@ -15,7 +15,6 @@ public AutoShoot5RamTotes(Drivetrain drivetrain) { new PathConstraints[] {new PathConstraints(5, 2)}, true ); - System.out.println(pathCommands); addCommands( pathCommands.get(0), pathCommands.get(1), diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOSim.java b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOSim.java index d7b301b..f0e23e6 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOSim.java +++ b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOSim.java @@ -13,7 +13,7 @@ public class ShooterIOSim implements ShooterIO { private final PIDController pid = new PIDController(ShooterConstants.KP, ShooterConstants.KI, ShooterConstants.KD); private final SimpleMotorFeedforward feedforward = - new SimpleMotorFeedforward(ShooterConstants.KS, ShooterConstants.KV); + new SimpleMotorFeedforward(0, ShooterConstants.KV); // no kS since sim doesn't have friction private double appliedVolts = 0; private boolean isClosedLoop = false; @@ -23,10 +23,11 @@ public class ShooterIOSim implements ShooterIO { public void updateInputs(ShooterInputs inputs) { if (isClosedLoop) { appliedVolts = - pid.calculate(sim.getAngularVelocityRPM() * 60, closedLoopSetpoint) + pid.calculate(sim.getAngularVelocityRPM() / 60, closedLoopSetpoint) + feedforward.calculate(closedLoopSetpoint); - setVoltage(appliedVolts); + sim.setInputVoltage(MathUtil.clamp(appliedVolts, -12, 12)); } + sim.update(0.02); inputs.velocityRPM = sim.getAngularVelocityRPM(); inputs.appliedVolts = appliedVolts; @@ -42,6 +43,7 @@ public void setVoltage(double volts) { @Override public void setVelocity(double speedRPM) { + pid.reset(); isClosedLoop = true; closedLoopSetpoint = speedRPM / 60; } From c87e90451aa59bbc924b867e302373f8f8831bee Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Tue, 5 Dec 2023 17:19:15 -0800 Subject: [PATCH 4/9] fix: ShooterSpinUpCommand now requires the subsystem --- .../bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java index 46addf1..6135114 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java @@ -9,6 +9,7 @@ public class ShooterSpinUpCommand extends CommandBase { public ShooterSpinUpCommand(Shooter shooter, double rpm) { this.shooter = shooter; this.setpoint = rpm; + addRequirements(shooter); } @Override From 6e632f19d774962c0f6c1356102080c88bc9b5a1 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 8 Dec 2023 09:13:16 -0800 Subject: [PATCH 5/9] feat: shooter idle speed --- .../team1540/bunnybotTank2023/Constants.java | 1 + .../bunnybotTank2023/RobotContainer.java | 12 ++++++---- .../shooter/ShooterSpinUpCommand.java | 24 ------------------- 3 files changed, 8 insertions(+), 29 deletions(-) delete mode 100644 src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java diff --git a/src/main/java/org/team1540/bunnybotTank2023/Constants.java b/src/main/java/org/team1540/bunnybotTank2023/Constants.java index ed576bf..b127c68 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/Constants.java +++ b/src/main/java/org/team1540/bunnybotTank2023/Constants.java @@ -63,6 +63,7 @@ public static class ShooterConstants { public static final double KV = 0.1149; public static final double ERROR_TOLERANCE_RPM = 30; + public static final double SHOOTER_IDLE_RPM = 1000; public static final double MOI = 0.0014924622; diff --git a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java index 72cb863..5f0a660 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java +++ b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java @@ -6,9 +6,8 @@ package org.team1540.bunnybotTank2023; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.team1540.bunnybotTank2023.commands.auto.AutoShoot5RamTotes; import org.team1540.bunnybotTank2023.commands.drivetrain.Drivetrain; import org.team1540.bunnybotTank2023.commands.drivetrain.TankdriveCommand; @@ -18,6 +17,8 @@ import org.team1540.bunnybotTank2023.io.shooter.ShooterIOReal; import org.team1540.bunnybotTank2023.io.shooter.ShooterIOSim; +import static org.team1540.bunnybotTank2023.Constants.*; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -33,8 +34,6 @@ public class RobotContainer { CommandXboxController driver = new CommandXboxController(0); CommandXboxController copilot = new CommandXboxController(1); - LoggedDashboardNumber shooterRPM = new LoggedDashboardNumber("shooterRPM", 0); - public RobotContainer() { if (Robot.isReal()) { // Initialize subsystems with hardware IO @@ -52,11 +51,14 @@ public RobotContainer() { /** Use this method to define your trigger->command mappings. */ private void configureButtonBindings() { - driver.a().onTrue(new InstantCommand(() -> shooter.setVelocity(shooterRPM.get()))).onFalse(new InstantCommand(() -> shooter.stop())); } private void setDefaultCommands() { drivetrain.setDefaultCommand(new TankdriveCommand(drivetrain, driver)); + shooter.setDefaultCommand(new StartEndCommand( + () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM), + () -> shooter.stop() + )); } diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java deleted file mode 100644 index 6135114..0000000 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShooterSpinUpCommand.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.team1540.bunnybotTank2023.commands.shooter; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ShooterSpinUpCommand extends CommandBase { - private final Shooter shooter; - private final double setpoint; - - public ShooterSpinUpCommand(Shooter shooter, double rpm) { - this.shooter = shooter; - this.setpoint = rpm; - addRequirements(shooter); - } - - @Override - public void initialize() { - shooter.setVelocity(setpoint); - } - - @Override - public boolean isFinished() { - return shooter.isAtSetpoint(); - } -} From db12094983fcc0048d6bc341b6417aaa0f1227e7 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 8 Dec 2023 09:18:54 -0800 Subject: [PATCH 6/9] fix: shooter default command requires shooter --- .../java/org/team1540/bunnybotTank2023/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java index 5f0a660..a49fa95 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java +++ b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java @@ -57,7 +57,8 @@ private void setDefaultCommands() { drivetrain.setDefaultCommand(new TankdriveCommand(drivetrain, driver)); shooter.setDefaultCommand(new StartEndCommand( () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM), - () -> shooter.stop() + () -> shooter.stop(), + shooter )); } From f4c8d90ca4f02d36e17ac1ada90086c537eed961 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 8 Dec 2023 22:13:38 -0800 Subject: [PATCH 7/9] refactor: move motor configs to separate file --- .../bunnybotTank2023/CTREConfigs.java | 36 +++++++++++++++++++ .../team1540/bunnybotTank2023/Constants.java | 30 ---------------- .../io/shooter/ShooterIOReal.java | 5 +-- 3 files changed, 39 insertions(+), 32 deletions(-) create mode 100644 src/main/java/org/team1540/bunnybotTank2023/CTREConfigs.java diff --git a/src/main/java/org/team1540/bunnybotTank2023/CTREConfigs.java b/src/main/java/org/team1540/bunnybotTank2023/CTREConfigs.java new file mode 100644 index 0000000..bf9c985 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/CTREConfigs.java @@ -0,0 +1,36 @@ +package org.team1540.bunnybotTank2023; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class CTREConfigs { + public static final TalonFXConfiguration shooterMotorConfig = new TalonFXConfiguration(); + + static { + /* Shooter motor config */ + MotorOutputConfigs shooterOutput = new MotorOutputConfigs(); + shooterOutput.Inverted = InvertedValue.CounterClockwise_Positive; + shooterOutput.NeutralMode = NeutralModeValue.Coast; + + CurrentLimitsConfigs shooterCurrentLimit = new CurrentLimitsConfigs(); + shooterCurrentLimit.SupplyCurrentLimitEnable = true; + shooterCurrentLimit.SupplyCurrentLimit = 40; + shooterCurrentLimit.SupplyCurrentThreshold = 60; + shooterCurrentLimit.SupplyTimeThreshold = 0.1; + + Slot0Configs shooterPID = new Slot0Configs(); + shooterPID.kP = Constants.ShooterConstants.KP; + shooterPID.kI = Constants.ShooterConstants.KI; + shooterPID.kD = Constants.ShooterConstants.KD; + shooterPID.kS = Constants.ShooterConstants.KS; + shooterPID.kV = Constants.ShooterConstants.KV; + + shooterMotorConfig.MotorOutput = shooterOutput; + shooterMotorConfig.CurrentLimits = shooterCurrentLimit; + shooterMotorConfig.Slot0 = shooterPID; + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/Constants.java b/src/main/java/org/team1540/bunnybotTank2023/Constants.java index b127c68..51a0f8a 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/Constants.java +++ b/src/main/java/org/team1540/bunnybotTank2023/Constants.java @@ -1,11 +1,5 @@ package org.team1540.bunnybotTank2023; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; public final class Constants { @@ -66,29 +60,5 @@ public static class ShooterConstants { public static final double SHOOTER_IDLE_RPM = 1000; public static final double MOI = 0.0014924622; - - public static final TalonFXConfiguration MOTOR_CONFIG = new TalonFXConfiguration(); - static { - MotorOutputConfigs outputConfigs = new MotorOutputConfigs(); - outputConfigs.Inverted = InvertedValue.CounterClockwise_Positive; - outputConfigs.NeutralMode = NeutralModeValue.Coast; - - CurrentLimitsConfigs currentLimitConfigs = new CurrentLimitsConfigs(); - currentLimitConfigs.SupplyCurrentLimitEnable = true; - currentLimitConfigs.SupplyCurrentLimit = 40; - currentLimitConfigs.SupplyCurrentThreshold = 60; - currentLimitConfigs.SupplyTimeThreshold = 0.1; - - Slot0Configs pidConfigs = new Slot0Configs(); - pidConfigs.kP = KP; - pidConfigs.kI = KI; - pidConfigs.kD = KD; - pidConfigs.kS = KS; - pidConfigs.kV = KV; - - MOTOR_CONFIG.MotorOutput = outputConfigs; - MOTOR_CONFIG.CurrentLimits = currentLimitConfigs; - MOTOR_CONFIG.Slot0 = pidConfigs; - } } } diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java index 1b2cd97..de2596e 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java +++ b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import org.team1540.bunnybotTank2023.CTREConfigs; import static org.team1540.bunnybotTank2023.Constants.*; @@ -19,8 +20,8 @@ public class ShooterIOReal implements ShooterIO{ private final StatusSignal current = leftMotor.getSupplyCurrent(); public ShooterIOReal() { - leftMotor.getConfigurator().apply(ShooterConstants.MOTOR_CONFIG); - rightMotor.getConfigurator().apply(ShooterConstants.MOTOR_CONFIG); + leftMotor.getConfigurator().apply(CTREConfigs.shooterMotorConfig); + rightMotor.getConfigurator().apply(CTREConfigs.shooterMotorConfig); rightMotor.setControl(new Follower(ShooterConstants.LEADER_ID, true)); } From c5b4b81abce505f569456b6e9ae67e16cf352285 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 8 Dec 2023 22:38:05 -0800 Subject: [PATCH 8/9] fix: stop creating new control requests --- .../bunnybotTank2023/io/shooter/ShooterIOReal.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java index de2596e..b13f313 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java +++ b/src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java @@ -19,6 +19,9 @@ public class ShooterIOReal implements ShooterIO{ private final StatusSignal dutyCycle = leftMotor.getDutyCycle(); private final StatusSignal current = leftMotor.getSupplyCurrent(); + private final VelocityVoltage velocityControlRequest = new VelocityVoltage(0).withSlot(0); + private final VoltageOut voltageControlRequest = new VoltageOut(0); + public ShooterIOReal() { leftMotor.getConfigurator().apply(CTREConfigs.shooterMotorConfig); rightMotor.getConfigurator().apply(CTREConfigs.shooterMotorConfig); @@ -34,13 +37,12 @@ public void updateInputs(ShooterInputs inputs) { @Override public void setVoltage(double volts) { - leftMotor.setControl(new VoltageOut(volts)); + leftMotor.setControl(voltageControlRequest.withOutput(volts)); } @Override public void setVelocity(double speedRPM) { - leftMotor.setControl(new VelocityVoltage(speedRPM / 60)); - System.out.println("setting rpm"); + leftMotor.setControl(velocityControlRequest.withVelocity(speedRPM / 60)); } @Override From cc742aeb95b72d43235549941bb49658f4197ff9 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sun, 10 Dec 2023 10:45:41 -0800 Subject: [PATCH 9/9] fix: remove unecessary code --- src/main/java/org/team1540/bunnybotTank2023/Robot.java | 1 - .../team1540/bunnybotTank2023/commands/shooter/Shooter.java | 4 ---- 2 files changed, 5 deletions(-) diff --git a/src/main/java/org/team1540/bunnybotTank2023/Robot.java b/src/main/java/org/team1540/bunnybotTank2023/Robot.java index 2409d1c..2b4f0cf 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/Robot.java +++ b/src/main/java/org/team1540/bunnybotTank2023/Robot.java @@ -98,7 +98,6 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } - robotContainer.shooter.stop(); } @Override diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java index 6d32c77..90642de 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java @@ -21,7 +21,6 @@ public class Shooter extends SubsystemBase { private final AverageFilter averageFilter = new AverageFilter(20); // TODO: 11/28/2023 tune filter size private double setpoint = 0; - private boolean isClosedLoop = false; public Shooter(ShooterIO io) { this.io = io; @@ -43,13 +42,11 @@ public void periodic() { public void setVelocity(double speedRPM) { setpoint = speedRPM; - isClosedLoop = true; averageFilter.clear(); io.setVelocity(speedRPM); } public void setVoltage(double volts) { - isClosedLoop = false; io.setVoltage(volts); } @@ -58,7 +55,6 @@ public void stop() { } public boolean isAtSetpoint() { - if (!isClosedLoop) return false; return Math.abs(averageFilter.getAverage() - setpoint) < ShooterConstants.ERROR_TOLERANCE_RPM; } }