Intake Feeder Wheel

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

@@ -11,10 +11,12 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
 import frc.robot.generated.TunerConstants;
@@ -24,6 +26,9 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
+import frc.robot.subsystems.intakeFeederwheel.IntakeFeederwheelIO;
+import frc.robot.subsystems.intakeFeederwheel.IntakeFeederwheelSubsystem;
+import frc.robot.subsystems.intakeFeederwheel.IntakeFeederwheelTalonFX;
 import frc.robot.subsystems.flywheel.FlywheelIO;
 import frc.robot.subsystems.flywheel.FlywheelIOTalonFX;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
@@ -37,6 +42,7 @@
 public class RobotContainer {
     // Subsystems
     private final Drive drive;
+    private IntakeFeederwheelSubsystem intakeFeederwheel;
     private final FlywheelIO flywheel;
 
     // Controller
@@ -75,6 +81,8 @@ public RobotContainer() {
                 //         new ModuleIOTalonFXS(TunerConstants.BackLeft),
                 //         new ModuleIOTalonFXS(TunerConstants.BackRight));
 
+                intakeFeederwheel = new IntakeFeederwheelSubsystem(new IntakeFeederwheelTalonFX());
+
                 flywheel = new FlywheelIOTalonFX();
                 break;
 
@@ -88,6 +96,8 @@ public RobotContainer() {
                                 new ModuleIOSim(TunerConstants.BackLeft),
                                 new ModuleIOSim(TunerConstants.BackRight));
 
+                intakeFeederwheel = new IntakeFeederwheelSubsystem(new IntakeFeederwheelTalonFX());
+
                 flywheel = new FlywheelIOTalonFX();
                 break;
 
@@ -101,6 +111,8 @@ public RobotContainer() {
                                 new ModuleIO() {},
                                 new ModuleIO() {});
 
+                intakeFeederwheel = new IntakeFeederwheelSubsystem(new IntakeFeederwheelIO() {});
+
                 flywheel = new FlywheelIO() {};
                 break;
         }
@@ -134,9 +146,8 @@ public RobotContainer() {
 
     /**
      * Use this method to define your button->command mappings. Buttons can be created by
-     * instantiating a {@link GenericHID} or one of its subclasses ({@link
-     * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
-     * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     * instantiating a {@link GenericHID} or one of its subclasses ({@link Joystick} or {@link
+     * XboxController}), and then passing it to a {@link JoystickButton}.
      */
     private void configureButtonBindings() {
         // Default command, normal field-relative drive
@@ -172,6 +183,8 @@ private void configureButtonBindings() {
                                                                 Rotation2d.kZero)),
                                         drive)
                                 .ignoringDisable(true));
+
+        controller.rightTrigger().onTrue(intakeFeederwheel.rollIn());
         controller.leftTrigger().whileTrue(Commands.run(() -> flywheel.setRoller(1)));
     }
 

src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelConfig.java

@@ -0,0 +1,5 @@
+package frc.robot.subsystems.intakeFeederwheel;
+
+public class IntakeFeederwheelConfig {
+    static final int FeederwheelMotorID = 20;
+}

src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelIO.java

@@ -0,0 +1,15 @@
+package frc.robot.subsystems.intakeFeederwheel;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeFeederwheelIO {
+    @AutoLog
+    public static class IntakeFeederwheelIOInputs {
+        public double voltage = 0.0;
+        public double RPM = 0.0;
+    }
+
+    public default void updateInputs(IntakeFeederwheelIOInputs inputs) {}
+
+    public default void setPower(double power) {}
+}

src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelSubsystem.java

@@ -0,0 +1,24 @@
+package frc.robot.subsystems.intakeFeederwheel;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class IntakeFeederwheelSubsystem extends SubsystemBase {
+    private IntakeFeederwheelIO io;
+    private IntakeFeederwheelIOInputsAutoLogged inputs = new IntakeFeederwheelIOInputsAutoLogged();
+
+    public IntakeFeederwheelSubsystem(IntakeFeederwheelIO io) {
+        this.io = io;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Intake Feederwheel", inputs);
+    }
+
+    public Command rollIn() {
+        return runEnd(() -> io.setPower(0.5), () -> io.setPower(0));
+    }
+}

src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelTalonFX.java

@@ -0,0 +1,30 @@
+package frc.robot.subsystems.intakeFeederwheel;
+
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import frc.robot.Robot;
+import frc.robot.util.sim.PhysicsSim;
+
+public class IntakeFeederwheelTalonFX implements IntakeFeederwheelIO {
+    private TalonFX feederwheelMotor;
+
+    private final VoltageOut voltageRequest = new VoltageOut(0);
+
+    public IntakeFeederwheelTalonFX() {
+        feederwheelMotor = new TalonFX(IntakeFeederwheelConfig.FeederwheelMotorID);
+        if (Robot.isSimulation()) {
+            PhysicsSim.getInstance().addTalonFX(feederwheelMotor);
+        }
+    }
+
+    @Override
+    public void updateInputs(IntakeFeederwheelIOInputs inputs) {
+        inputs.voltage = feederwheelMotor.getMotorVoltage().getValueAsDouble();
+        inputs.RPM = feederwheelMotor.getVelocity().getValueAsDouble();
+    }
+
+    @Override
+    public void setPower(double volts) {
+        feederwheelMotor.setControl(voltageRequest.withOutput(volts));
+    }
+}

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

@@ -0,0 +1,11 @@
+package frc.robot.util.sim;
+
+import edu.wpi.first.epilogue.Logged;
+
+@Logged
+public class Mechanisms {
+
+    public Mechanisms() {}
+
+    public void publishComponentPoses() {}
+}

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

@@ -0,0 +1,9 @@
+package frc.robot.util.sim;
+
+import edu.wpi.first.units.measure.Angle;
+
+public interface SimulatableMechanism {
+    Angle getCurrentPosition();
+
+    Angle getTargetPosition();
+}