made flywheel(shooter) subsystem

src/main/java/frc/robot/Robot.java

@@ -137,4 +137,3 @@ public void simulationInit() {}
     @Override
     public void simulationPeriodic() {}
 }
-

src/main/java/frc/robot/RobotContainer.java

@@ -24,6 +24,8 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
+import frc.robot.subsystems.flywheel.FlywheelIO;
+import frc.robot.subsystems.flywheel.FlywheelIOTalonFX;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -35,6 +37,7 @@
 public class RobotContainer {
     // Subsystems
     private final Drive drive;
+    private final FlywheelIO flywheel;
 
     // Controller
     private final CommandXboxController controller = new CommandXboxController(0);
@@ -71,6 +74,8 @@ public RobotContainer() {
                 //         new ModuleIOTalonFXS(TunerConstants.FrontRight),
                 //         new ModuleIOTalonFXS(TunerConstants.BackLeft),
                 //         new ModuleIOTalonFXS(TunerConstants.BackRight));
+
+                flywheel = new FlywheelIOTalonFX();
                 break;
 
             case SIM:
@@ -82,6 +87,8 @@ public RobotContainer() {
                                 new ModuleIOSim(TunerConstants.FrontRight),
                                 new ModuleIOSim(TunerConstants.BackLeft),
                                 new ModuleIOSim(TunerConstants.BackRight));
+
+                flywheel = new FlywheelIOTalonFX();
                 break;
 
             default:
@@ -93,6 +100,8 @@ public RobotContainer() {
                                 new ModuleIO() {},
                                 new ModuleIO() {},
                                 new ModuleIO() {});
+
+                flywheel = new FlywheelIO() {};
                 break;
         }
 
@@ -163,6 +172,7 @@ private void configureButtonBindings() {
                                                                 Rotation2d.kZero)),
                                         drive)
                                 .ignoringDisable(true));
+        controller.leftTrigger().whileTrue(Commands.run(() -> flywheel.setRoller(1)));
     }
 
     /**

src/main/java/frc/robot/subsystems/flywheel/Flywheel.java

@@ -0,0 +1,31 @@
+package frc.robot.subsystems.flywheel;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Flywheel extends SubsystemBase {
+    // Auto logged classes for AKit
+    private final FlywheelIO io;
+    private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged();
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Flywheel", inputs);
+    }
+
+    public Flywheel(FlywheelIO io) {
+        this.io = io;
+    }
+
+    public Command setRoller(double velocity) {
+        return runEnd(
+                () -> {
+                    io.setRoller(velocity);
+                },
+                () -> {
+                    io.setRoller(0);
+                });
+    }
+}

src/main/java/frc/robot/subsystems/flywheel/FlywheelConfigs.java

@@ -0,0 +1,16 @@
+package frc.robot.subsystems.flywheel;
+
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+public class FlywheelConfigs {
+    static final int leftMotorID = 41;
+    static final int rightMotorID = 67;
+
+    static TalonFXConfiguration rollerConfig =
+            new TalonFXConfiguration()
+                    .withMotorOutput(
+                            new MotorOutputConfigs()
+                                    .withInverted(InvertedValue.Clockwise_Positive));
+}

src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java

@@ -0,0 +1,19 @@
+package frc.robot.subsystems.flywheel;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface FlywheelIO {
+
+    @AutoLog
+    public static class FlywheelIOInputs {
+        public double leftMotorVelocityRPM = 0;
+        public double leftMotorSpeed = 0;
+
+        public double rightMotorVelocityRPM = 0;
+        public double rightMotorSpeed = 0;
+    }
+
+    public default void updateInputs(FlywheelIOInputs inputs) {}
+
+    public default void setRoller(double velocity) {}
+}

src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java

@@ -0,0 +1,42 @@
+package frc.robot.subsystems.flywheel;
+
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import frc.robot.Robot;
+import frc.robot.util.sim.PhysicsSim;
+
+public class FlywheelIOTalonFX implements FlywheelIO {
+    private final TalonFX leftMotor;
+    private final TalonFX rightMotor;
+
+    private final MotionMagicVelocityVoltage motionRequest = new MotionMagicVelocityVoltage(0);
+
+    public FlywheelIOTalonFX() {
+        leftMotor = new TalonFX(FlywheelConfigs.leftMotorID);
+        rightMotor = new TalonFX(FlywheelConfigs.rightMotorID);
+        // make left a follower of right
+        leftMotor.setControl(new Follower(rightMotor.getDeviceID(), false));
+        leftMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig);
+        rightMotor.getConfigurator().apply(FlywheelConfigs.rollerConfig);
+        if (Robot.isSimulation()) {
+            PhysicsSim.getInstance().addTalonFX(leftMotor);
+            PhysicsSim.getInstance().addTalonFX(rightMotor);
+        }
+    }
+
+    @Override
+    public void updateInputs(FlywheelIO.FlywheelIOInputs inputs) {
+        inputs.leftMotorVelocityRPM = leftMotor.getVelocity().getValueAsDouble();
+        inputs.leftMotorSpeed = leftMotor.getDutyCycle().getValueAsDouble();
+
+        inputs.rightMotorVelocityRPM = rightMotor.getVelocity().getValueAsDouble();
+        inputs.rightMotorSpeed = rightMotor.getDutyCycle().getValueAsDouble();
+    }
+
+    @Override
+    public void setRoller(double velocity) {
+        leftMotor.setControl(motionRequest.withVelocity(velocity));
+    }
+}

src/main/java/frc/robot/util/sim/PhysicsSim.java

@@ -0,0 +1,68 @@
+package frc.robot.util.sim;
+
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import java.util.ArrayList;
+
+/** Manages physics simulation for CTRE products. */
+public class PhysicsSim {
+    private static final PhysicsSim sim = new PhysicsSim();
+    private final ArrayList<SimProfile> simProfiles = new ArrayList<>();
+
+    /** Gets the robot simulator instance. */
+    public static PhysicsSim getInstance() {
+        return sim;
+    }
+
+    /**
+     * Adds a TalonFX controller to the simulator.
+     *
+     * @param talonFX The TalonFX device
+     */
+    public void addTalonFX(TalonFX talonFX) {
+        if (talonFX != null) {
+            TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, 0.001);
+            simProfiles.add(simTalonFX);
+        }
+    }
+
+    public void addTalonFX(TalonFX talonFX, CANcoder cancoder) {
+        if (talonFX != null && cancoder != null) {
+            TalonFXSimProfile simTalonFX = new TalonFXSimProfile(talonFX, 0.001, cancoder);
+            simProfiles.add(simTalonFX);
+        }
+    }
+
+    /** Runs the simulator: - enable the robot - simulate sensors */
+    public void run() {
+        // Simulate devices
+        for (SimProfile simProfile : simProfiles) {
+            simProfile.run();
+        }
+    }
+
+    /** Holds information about a simulated device. */
+    static class SimProfile {
+        private double _lastTime;
+        private boolean _running = false;
+
+        /** Runs the simulation profile. Implemented by device-specific profiles. */
+        public void run() {}
+
+        /** Returns the time since last call, in seconds. */
+        protected double getPeriod() {
+            // set the start time if not yet running
+            if (!_running) {
+                _lastTime = Utils.getCurrentTimeSeconds();
+                _running = true;
+            }
+
+            double now = Utils.getCurrentTimeSeconds();
+            final double period = now - _lastTime;
+            _lastTime = now;
+
+            return period;
+        }
+    }
+}

src/main/java/frc/robot/util/sim/TalonFXSimProfile.java

@@ -0,0 +1,67 @@
+package frc.robot.util.sim;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.sim.CANcoderSimState;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+
+/** Holds information about a simulated TalonFX. */
+class TalonFXSimProfile extends PhysicsSim.SimProfile {
+    private static final double MOTOR_RESISTANCE =
+            0.002; // Assume 2mOhm resistance for voltage drop calculation
+
+    private final DCMotorSim motorSim;
+    private final TalonFXSimState talonFXSim;
+    private CANcoderSimState cancoderSimState;
+
+    /**
+     * Creates a new simulation profile for a TalonFX device.
+     *
+     * @param talonFX The TalonFX device
+     * @param rotorInertia Rotational Inertia of the mechanism at the rotor
+     */
+    public TalonFXSimProfile(final TalonFX talonFX, final double rotorInertia) {
+        var gearbox = DCMotor.getKrakenX60Foc(1);
+        this.motorSim =
+                new DCMotorSim(
+                        LinearSystemId.createDCMotorSystem(gearbox, rotorInertia, 1.0), gearbox);
+        this.talonFXSim = talonFX.getSimState();
+    }
+
+    public TalonFXSimProfile(final TalonFX talonFX, final double rotorInertia, CANcoder cancoder) {
+        this(talonFX, rotorInertia);
+        cancoderSimState = cancoder.getSimState();
+    }
+
+    /**
+     * Runs the simulation profile.
+     *
+     * <p>This uses very rudimentary physics simulation and exists to allow users to test features
+     * of our products in simulation using our examples out of the box. Users may modify this to
+     * utilize more accurate physics simulation.
+     */
+    public void run() {
+        /// DEVICE SPEED SIMULATION
+
+        motorSim.setInputVoltage(talonFXSim.getMotorVoltage());
+
+        motorSim.update(getPeriod());
+
+        /// SET SIM PHYSICS INPUTS
+        final double position_rot = motorSim.getAngularPositionRotations();
+        final double velocity_rps = Units.radiansToRotations(motorSim.getAngularVelocityRPM());
+
+        if (cancoderSimState != null) {
+            cancoderSimState.setRawPosition(position_rot);
+            cancoderSimState.setVelocity(velocity_rps);
+        }
+        talonFXSim.setRawRotorPosition(position_rot);
+        talonFXSim.setRotorVelocity(velocity_rps);
+
+        talonFXSim.setSupplyVoltage(12 - talonFXSim.getSupplyCurrent() * MOTOR_RESISTANCE);
+    }
+}