Shooter code

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;
+    }
+}

src/main/java/org/team1540/bunnybotTank2023/Constants.java

@@ -3,6 +3,9 @@
 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 = true;
+
     // Simulation mode, irrelevant for code running on physical robot
     public static final SimulationMode simulationMode = SimulationMode.SIM;
 
@@ -42,4 +45,20 @@ 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.6; // TODO: 11/30/2023 tuned, doesn't work well for values < 1000 rpm
+        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;
+
+        public static final double ERROR_TOLERANCE_RPM = 30;
+        public static final double SHOOTER_IDLE_RPM = 1000;
+
+        public static final double MOI = 0.0014924622;
+    }
 }

src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java

@@ -6,12 +6,18 @@
 package org.team1540.bunnybotTank2023;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 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;
+
+import static org.team1540.bunnybotTank2023.Constants.*;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -22,6 +28,7 @@
 public class RobotContainer {
     // Subsystems
     Drivetrain drivetrain;
+    Shooter shooter;
 
     // Controllers
     CommandXboxController driver = new CommandXboxController(0);
@@ -31,9 +38,11 @@ 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,11 +51,15 @@ public RobotContainer() {
 
     /** Use this method to define your trigger->command mappings. */
     private void configureButtonBindings() {
-
     }
 
     private void setDefaultCommands() {
         drivetrain.setDefaultCommand(new TankdriveCommand(drivetrain, driver));
+        shooter.setDefaultCommand(new StartEndCommand(
+                () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM),
+                () -> shooter.stop(),
+                shooter
+        ));
     }
 
 

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),

src/main/java/org/team1540/bunnybotTank2023/commands/shooter/Shooter.java

@@ -1,6 +1,60 @@
 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;
+
+    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;
+        averageFilter.clear();
+        io.setVelocity(speedRPM);
+    }
+
+    public void setVoltage(double volts) {
+        io.setVoltage(volts);
+    }
+
+    public void stop() {
+        setVoltage(0);
+    }
+
+    public boolean isAtSetpoint() {
+        return Math.abs(averageFilter.getAverage() - setpoint) < ShooterConstants.ERROR_TOLERANCE_RPM;
+    }
 }

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);
+}

src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOReal.java

@@ -0,0 +1,57 @@
+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 org.team1540.bunnybotTank2023.CTREConfigs;
+
+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<Double> velocity = leftMotor.getVelocity();
+    private final StatusSignal<Double> busVoltage = leftMotor.getSupplyVoltage();
+    private final StatusSignal<Double> dutyCycle = leftMotor.getDutyCycle();
+    private final StatusSignal<Double> 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);
+        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(voltageControlRequest.withOutput(volts));
+    }
+
+    @Override
+    public void setVelocity(double speedRPM) {
+        leftMotor.setControl(velocityControlRequest.withVelocity(speedRPM / 60));
+    }
+
+    @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);
+    }
+}

src/main/java/org/team1540/bunnybotTank2023/io/shooter/ShooterIOSim.java

@@ -0,0 +1,55 @@
+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(0, ShooterConstants.KV); // no kS since sim doesn't have friction
+
+    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);
+            sim.setInputVoltage(MathUtil.clamp(appliedVolts, -12, 12));
+        }
+        sim.update(0.02);
+
+        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) {
+        pid.reset();
+        isClosedLoop = true;
+        closedLoopSetpoint = speedRPM / 60;
+    }
+
+    @Override
+    public void configurePID(double kP, double kI, double kD) {
+        pid.setPID(kP, kI, kD);
+    }
+}

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;
+    }
+}