diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1ac7323..5b2c31f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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))); } diff --git a/src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelConfig.java b/src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelConfig.java new file mode 100644 index 0000000..a904b5b --- /dev/null +++ b/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; +} diff --git a/src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelIO.java b/src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelIO.java new file mode 100644 index 0000000..c7f2dcb --- /dev/null +++ b/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) {} +} diff --git a/src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelSubsystem.java b/src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelSubsystem.java new file mode 100644 index 0000000..08cccac --- /dev/null +++ b/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)); + } +} diff --git a/src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelTalonFX.java b/src/main/java/frc/robot/subsystems/intakeFeederwheel/IntakeFeederwheelTalonFX.java new file mode 100644 index 0000000..7f289ca --- /dev/null +++ b/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)); + } +} diff --git a/src/main/java/frc/robot/util/sim/Mechanisms.java b/src/main/java/frc/robot/util/sim/Mechanisms.java new file mode 100644 index 0000000..8934f6e --- /dev/null +++ b/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() {} +} diff --git a/src/main/java/frc/robot/util/sim/SimulatableMechanism.java b/src/main/java/frc/robot/util/sim/SimulatableMechanism.java new file mode 100644 index 0000000..7e3f2ff --- /dev/null +++ b/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(); +}