Did the same thing as the other one

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

@@ -13,7 +13,6 @@
 
 package frc.robot;
 
-import com.ctre.phoenix6.hardware.TalonFX;
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -23,14 +22,26 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.lib.W8.io.absoluteencoder.AbsoluteEncoderIOCANCoderSim;
+import frc.lib.W8.io.motor.*;
+import frc.lib.W8.mechanisms.flywheel.*;
+import frc.lib.W8.mechanisms.rotary.RotaryMechanism;
+import frc.lib.W8.mechanisms.rotary.RotaryMechanismReal;
+import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim;
+import frc.robot.Constants.HopperConstants;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.Constants.Ports;
 import frc.robot.commands.DriveCommands;
 import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.Hopper;
+import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
+import java.util.Optional;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -42,6 +53,8 @@
 public class RobotContainer {
   // Subsystems
   private final Drive drive;
+  private final Hopper hopper;
+  private final Intake intake;
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -61,6 +74,33 @@ public RobotContainer() {
                 new ModuleIOTalonFX(TunerConstants.FrontRight),
                 new ModuleIOTalonFX(TunerConstants.BackLeft),
                 new ModuleIOTalonFX(TunerConstants.BackRight));
+
+        hopper =
+            new Hopper(
+                new FlywheelMechanismReal(
+                    new MotorIOTalonFX(
+                        HopperConstants.MOTOR_NAME,
+                        HopperConstants.getFXConfig(),
+                        Ports.HopperRoller)));
+
+        intake =
+            new Intake(
+                new FlywheelMechanismReal(
+                    new MotorIOTalonFX(
+                        IntakeConstants.MOTOR_NAME,
+                        IntakeConstants.getFXConfig(),
+                        Ports.IntakeRoller)),
+                new RotaryMechanismReal(
+                    new MotorIOTalonFX(
+                        IntakeConstants.MOTOR_NAME,
+                        IntakeConstants.getFXConfig(),
+                        Ports.IntakeRoller),
+                    IntakeConstants.CONSTANTS,
+                    Optional.of(
+                        new AbsoluteEncoderIOCANCoderSim(
+                            Ports.IntakeRoller,
+                            IntakeConstants.MOTOR_NAME + " Encoder",
+                            IntakeConstants.getCANcoderConfig(false)))));
         break;
 
       case SIM:
@@ -72,6 +112,42 @@ public RobotContainer() {
                 new ModuleIOSim(TunerConstants.FrontRight),
                 new ModuleIOSim(TunerConstants.BackLeft),
                 new ModuleIOSim(TunerConstants.BackRight));
+
+        hopper =
+            new Hopper(
+                new FlywheelMechanismSim(
+                    new MotorIOTalonFXSim(
+                        HopperConstants.MOTOR_NAME,
+                        HopperConstants.getFXConfig(),
+                        Ports.HopperRoller),
+                    HopperConstants.DCMOTOR,
+                    HopperConstants.MOI,
+                    HopperConstants.TOLERANCE));
+
+        intake =
+            new Intake(
+                new FlywheelMechanismSim(
+                    new MotorIOTalonFXSim(
+                        IntakeConstants.MOTOR_NAME,
+                        IntakeConstants.getFXConfig(),
+                        Ports.IntakeRoller),
+                    IntakeConstants.DCMOTOR,
+                    IntakeConstants.MOI,
+                    IntakeConstants.TOLERANCE),
+                new RotaryMechanismSim(
+                    new MotorIOTalonFXSim(
+                        IntakeConstants.MOTOR_NAME,
+                        IntakeConstants.getFXConfig(),
+                        Ports.IntakeRoller),
+                    IntakeConstants.DCMOTOR,
+                    IntakeConstants.MOI,
+                    false,
+                    IntakeConstants.CONSTANTS,
+                    Optional.of(
+                        new AbsoluteEncoderIOCANCoderSim(
+                            Ports.IntakeRoller,
+                            IntakeConstants.MOTOR_NAME + " Encoder",
+                            IntakeConstants.getCANcoderConfig(false)))));
         break;
 
       default:
@@ -83,6 +159,13 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {},
                 new ModuleIO() {});
+
+        hopper = new Hopper(new FlywheelMechanism() {});
+
+        intake =
+            new Intake(
+                new FlywheelMechanism() {},
+                new RotaryMechanism(IntakeConstants.MOTOR_NAME, IntakeConstants.CONSTANTS) {});
         break;
     }
 
@@ -147,6 +230,10 @@ private void configureButtonBindings() {
                             new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
                     drive)
                 .ignoringDisable(true));
+
+    controller
+        .x()
+        .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper));
   }
 
   /**
@@ -157,6 +244,4 @@ private void configureButtonBindings() {
   public Command getAutonomousCommand() {
     return autoChooser.get();
   }
-
-  TalonFX billyBob = new TalonFX(30, TunerConstants.DrivetrainConstants.CANBusName);
 }

src/main/java/frc/robot/subsystems/Climber.java

@@ -0,0 +1,30 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.W8.mechanisms.linear.LinearMechanism;
+import frc.robot.Constants.ClimberConstants;
+
+public class Climber extends SubsystemBase {
+  private LinearMechanism _io;
+
+  public Climber(LinearMechanism io) {
+    io = _io;
+  }
+
+  public enum State {
+    IDLE(Units.MetersPerSecond.of(0.0)),
+    ASCENDING(Units.MetersPerSecond.of(ClimberConstants.CLIMB_SPEED)),
+    DESCENDING(Units.MetersPerSecond.of(-ClimberConstants.CLIMB_SPEED));
+
+    private final LinearVelocity velocity;
+
+    private State(LinearVelocity velocity) {
+      this.velocity = velocity;
+    }
+  }
+
+  @Override
+  public void periodic() {}
+}

src/main/java/frc/robot/subsystems/Hopper.java

@@ -0,0 +1,45 @@
+package frc.robot.subsystems;
+
+import static edu.wpi.first.units.Units.*;
+import static edu.wpi.first.units.Units.Inches;
+
+import edu.wpi.first.units.measure.*;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.W8.io.motor.MotorIO.PIDSlot;
+import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism;
+import frc.robot.Constants.HopperConstants;
+
+public class Hopper extends SubsystemBase {
+  private FlywheelMechanism _io;
+
+  public enum State {
+    OFF(RevolutionsPerSecond.of(0.0)),
+    FORWARD_SLOW(RevolutionsPerSecond.of(HopperConstants.SLOW_SPEED_RPM / 60)),
+    FORWARD_FAST(RevolutionsPerSecond.of(HopperConstants.FAST_SPEED_RPM / 60)),
+    REVERSE(RevolutionsPerSecond.of(HopperConstants.REVERSE_SPEED_RPM / 60));
+
+    private final AngularVelocity _stateVelocity;
+
+    private State(AngularVelocity stateVelocity) {
+      _stateVelocity = stateVelocity;
+    }
+  }
+
+  public Hopper(FlywheelMechanism io) {
+    _io = io;
+  }
+
+  public void setGoal(double position) {
+    Distance positionInches = Inches.of(position);
+    _io.runPosition(
+        HopperConstants.CONVERTER.toAngle(positionInches),
+        HopperConstants.ANGULAR_VELOCITY,
+        HopperConstants.ANGULAR_ACCELERATION,
+        null,
+        PIDSlot.SLOT_0);
+  }
+
+  @Override
+  public void periodic() {}
+}

src/main/java/frc/robot/subsystems/intake/Intake.java → src/main/java/frc/robot/subsystems/Intake.java

@@ -1,4 +1,4 @@
-package frc.robot.subsystems.intake;
+package frc.robot.subsystems;
 
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
@@ -9,10 +9,12 @@
 import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism;
 import frc.lib.W8.mechanisms.rotary.RotaryMechanism;
 import frc.robot.Constants;
+import frc.robot.Constants.IntakePivotConstants;
 
 public class Intake extends SubsystemBase {
   private FlywheelMechanism _rollerIO;
   private RotaryMechanism _pivotIO;
+  public double desiredAngle;
 
   public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) {
     _rollerIO = rollerIO;
@@ -38,6 +40,21 @@ public void stop() {
     setVelocity(0);
   }
 
+  public void setAngle(Angle angle) {
+    _pivotIO.runPosition(
+        angle,
+        getVelocity(),
+        IntakePivotConstants.ACCELERATION,
+        IntakePivotConstants.JERK,
+        PIDSlot.SLOT_0);
+    desiredAngle = angle.magnitude();
+  }
+
+  public boolean isIntendedAngle() {
+    return Math.abs(desiredAngle - _pivotIO.getVelocity().in(RotationsPerSecond))
+        <= IntakePivotConstants.TOLERANCE.magnitude();
+  }
+
   @Override
   public void periodic() {}
 }

src/main/java/frc/robot/subsystems/LEDs.java

@@ -0,0 +1,27 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.controls.SolidColor;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.W8.devices.Lights;
+import frc.lib.W8.io.lights.LightsIO;
+import frc.robot.Constants.LEDConstants;
+
+public class LEDs extends SubsystemBase {
+  private final LightsIO _io;
+  private final Lights _lights;
+
+  public LEDs(LightsIO io, Lights lights) {
+    _io = io;
+    _lights = lights;
+  }
+
+  public Command runAnimation() {
+    return this.startEnd(
+        () -> _lights.setAnimation(new SolidColor(0, 3).withColor(LEDConstants.colorPaleBlue)),
+        () -> _lights.setAnimation(new SolidColor(0, 3).withColor(LEDConstants.colorWheezerBlue)));
+  }
+
+  @Override
+  public void periodic() {}
+}