Merge branch 'main' of https://github.com/team5115/2025-Bunnybots

Author: AvidCoder27

src/main/java/frc/team5115/subsystems/intake-wheel/IntakeWheel.java

@@ -0,0 +1,62 @@
+package frc.team5115.subsystems.intakewheel;
+
+import com.revrobotics.spark.SparkMax;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.ArrayList;
+import java.util.function.BooleanSupplier;
+import org.littletonrobotics.junction.Logger;
+
+public class IntakeWheel extends SubsystemBase {
+    private static final double INTAKE_SPEED = 0.15;
+    private final IntakeWheelIO io;
+    private final IntakeWheelIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+
+    public IntakeWheel(IntakeIO io) {
+        this.io = io;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Intake", inputs);
+    }
+
+    public Command setSpeed(double speed) {
+        return Commands.runOnce(() -> io.setPercent(speed));
+    }
+
+    public Command intake() {
+        return setSpeed(INTAKE_SPEED);
+    }
+
+    public Command vomit() {
+        return setSpeed(-0.22);
+    }
+
+    public Command stop() {
+        return setSpeed(0);
+    }
+
+    public void getSparks(ArrayList<SparkMax> sparks) {
+        io.getSparks(sparks);
+    }
+
+    public Command intakeIf(BooleanSupplier supplier) {
+        return Commands.run(
+                () -> {
+                    if (supplier.getAsBoolean()) {
+                        // stall means we back it up a little and try again
+                        if (inputs.currentAmps > 25.0) {
+                            io.setPercent(-0.22);
+                        } else {
+                            io.setPercent(INTAKE_SPEED);
+                        }
+                    } else {
+                        io.setPercent(0);
+                    }
+                },
+                this);
+    }
+}

src/main/java/frc/team5115/subsystems/intake-wheel/IntakeWheelIO.java

@@ -0,0 +1,26 @@
+package frc.team5115.subsystems.intakewheel;
+
+import com.revrobotics.spark.SparkMax;
+import java.util.ArrayList;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeWheelIO {
+    @AutoLog
+    public static class IntakeWheelIOInputs {
+        public double velocityRPM = 0.0;
+        public double appliedVolts = 0.0;
+        public double currentAmps = 0.0;
+    }
+
+    /** Updates the set of loggable inputs. */
+    public default void updateInputs(IntakeWheelIOInputs inputs) {}
+
+    /** Run the intake motor at the specified voltage. */
+    public default void setVoltage(double volts) {}
+
+    /** Run the intake motor at the specified percentage. */
+    public default void setPercent(double percent) {}
+
+    /** This should not really be here but it must be */
+    public default void getSparks(ArrayList<SparkMax> sparks) {}
+}

src/main/java/frc/team5115/subsystems/intake-wheel/IntakeWheelIOSim.java

@@ -0,0 +1,37 @@
+package frc.team5115.subsystems.intakewheel;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import frc.team5115.Constants;
+
+public class IntakeWheelIOSim implements IntakeWheelIO {
+    private final DCMotorSim sim;
+    private double appliedVolts;
+
+    public IntakeWheelIOSim() {
+        final DCMotor motor = DCMotor.getNEO(1);
+        sim = new DCMotorSim(LinearSystemId.createDCMotorSystem(motor, 0.0002, 1.0), motor);
+    }
+
+    @Override
+    public void updateInputs(IntakeWheelIOInputs inputs) {
+        sim.update(Constants.LOOP_PERIOD_SECS);
+        inputs.velocityRPM = sim.getAngularVelocityRPM();
+        inputs.appliedVolts = appliedVolts;
+        inputs.currentAmps = Math.abs(sim.getCurrentDrawAmps());
+    }
+
+    @Override
+    public void setVoltage(double volts) {
+        appliedVolts = MathUtil.clamp(volts, -12.0, +12.0);
+        sim.setInputVoltage(appliedVolts);
+    }
+
+    @Override
+    public void setPercent(double percent) {
+        appliedVolts = MathUtil.clamp(percent * 12, -12.0, +12.0);
+        sim.setInputVoltage(appliedVolts);
+    }
+}

src/main/java/frc/team5115/subsystems/intake-wheel/IntakeWheelIOSparkMax.java

@@ -0,0 +1,47 @@
+package frc.team5115.subsystems.intakewheel;
+
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import frc.team5115.Constants;
+import java.util.ArrayList;
+
+public class IntakeWheelIOSparkMax implements IntakeWheelIO {
+    private final SparkMax motor;
+    private final RelativeEncoder encoder;
+
+    public IntakeWheelIOSparkMax() {
+        motor = new SparkMax(Constants.INTAKE_MOTOR_ID, MotorType.kBrushless);
+        encoder = motor.getEncoder();
+
+        final SparkMaxConfig motorConfig = new SparkMaxConfig();
+        motorConfig.smartCurrentLimit(30, 40).idleMode(IdleMode.kCoast);
+        motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    @Override
+    public void updateInputs(IntakeWheelIOInputs inputs) {
+        inputs.velocityRPM = encoder.getVelocity();
+        inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage();
+        inputs.currentAmps = motor.getOutputCurrent();
+    }
+
+    @Override
+    public void setPercent(double percent) {
+        motor.set(percent);
+    }
+
+    @Override
+    public void setVoltage(double volts) {
+        motor.setVoltage(volts);
+    }
+
+    @Override
+    public void getSparks(ArrayList<SparkMax> sparks) {
+        sparks.add(motor);
+    }
+}

the evil table.txt

@@ -1,8 +1,10 @@
-Intake
+Intake Wheel
 - Single wheel - Motor
-- Arm - Motor
 - Spin wheel to intake, Move arm up to trade off, Spin wheel to trade.
 
+Intake Arm
+- Arm - Motor
+
 Defacator
 - Arm - Pnumatic
 - Wait till three Lunites (Sensor? Count Variable?), Move up arm