Intake bp

bp for intake with setmode method

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2026KPopRobotHunters";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 14;
-  public static final String GIT_SHA = "eb0d9de8d8342dc2b039ff2c80c2750776fa088f";
-  public static final String GIT_DATE = "2026-01-19 19:42:29 MST";
-  public static final String GIT_BRANCH = "Autorotate";
-  public static final String BUILD_DATE = "2026-01-20 19:19:56 MST";
-  public static final long BUILD_UNIX_TIME = 1768961996020L;
+  public static final int GIT_REVISION = 15;
+  public static final String GIT_SHA = "ba16f53d6d8155249e7ca767b54fbfa1f17ce68e";
+  public static final String GIT_DATE = "2026-01-20 19:23:14 MST";
+  public static final String GIT_BRANCH = "Intake-boilerplate";
+  public static final String BUILD_DATE = "2026-01-27 18:56:30 MST";
+  public static final long BUILD_UNIX_TIME = 1769565390510L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/subsystems/intake/Intake.java

@@ -0,0 +1,78 @@
+package frc4388.robot.subsystems.intake;
+
+import static edu.wpi.first.units.Units.Rotation;
+
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.utility.status.FaultReporter;
+
+public class Intake extends SubsystemBase {
+    IntakeIO io;
+    IntakeStateAutoLogged state = new IntakeStateAutoLogged();
+
+    Supplier<Pose2d> m_swervePoseSupplier;
+
+    public Intake(
+        IntakeIO io,
+        Supplier<Pose2d> swervePoseSupplier
+    ) {
+        this.io = io;
+        this.m_swervePoseSupplier = swervePoseSupplier;
+    }
+
+    public enum IntakeMode {
+        Up,
+        Down,
+    }
+
+    public void setMode(IntakeMode mode) {
+        switch (mode) {
+            case Up:
+                io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
+                break;
+            case Down:
+                io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
+                io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
+                break;
+        }
+    }
+
+
+    // public enum FieldZone {
+    //     // The robot should aim at the hub
+    //     InShootZone,
+    //     // The robot should aim towards the wall
+    //     AimAtWall,
+        
+    // }
+
+    // // Calculate what should be done based off of the position of the robot
+    // // TODO: Implement field zones
+    // public FieldZone getTarget(Pose2d position) {
+    //     return FieldZone.InShootZone;
+    // }
+
+    @Override
+    public void periodic() {
+        
+        
+
+        // FaultReporter.register(this); // TODO Implement fault reporter
+
+
+        Logger.processInputs("Intake", state);
+
+        Pose2d pose = m_swervePoseSupplier.get();
+        Angle robotRot = pose.getRotation().getMeasure();
+
+        io.updateInputs(state);
+
+    }
+}
+
+

src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java

@@ -0,0 +1,80 @@
+package frc4388.robot.subsystems.intake;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.Slot1Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import frc4388.utility.status.CanDevice;
+
+public class IntakeConstants {
+    // Motor conversions
+
+    public static final double ARM_MOTOR_GEAR_RATIO = 1.;
+    public static final double ROLLER_MOTOR_GEAR_RATIO = 1.;
+
+    // Limits
+
+    // 0 is the forward angle on the robot.
+    // negative is left, positive is right
+    public static final Angle ARM_LIMIT_LOWER = Degrees.of(-180);
+    public static final Angle ARM_LIMIT_UPPER = Degrees.of(180);
+    public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(0.0);
+
+    public static final Slot0Configs ARM_PID = new Slot0Configs()
+        .withKP(2.0)
+        .withKI(0.0)
+        .withKD(10.0);
+
+    public static final Slot1Configs ROLLER_PID = new Slot1Configs()
+        .withKP(2.0)
+        .withKI(0.0)
+        .withKD(10.0);
+
+    // 0 is paralell to the ground, 90 is directly up
+    // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
+    // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
+    
+    // Motor configs
+    public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(40) // TODO: tune???
+                .withStatorCurrentLimitEnable(true)
+            ).withMotorOutput(
+                new MotorOutputConfigs()
+                    .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
+                    .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
+    );
+
+    public static final class IDs {
+        public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22);
+    }
+    // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
+    //     .withCurrentLimits(
+    //         new CurrentLimitsConfigs()
+    //             .withStatorCurrentLimit(40) // TODO: tune???
+    //             .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
+    //         ).withMotorOutput(
+    //             new MotorOutputConfigs()
+    //                 .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop
+    //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
+    // );
+    public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(40) // TODO: tune???
+                .withStatorCurrentLimitEnable(true)
+            ).withMotorOutput(
+                new MotorOutputConfigs()
+                    .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
+                    .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
+    );
+}

src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java

@@ -0,0 +1,42 @@
+package frc4388.robot.subsystems.intake;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.InchesPerSecond;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.units.CurrentUnit;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.LinearVelocity;
+
+public interface IntakeIO {
+    @AutoLog
+    public class IntakeState {
+        Angle armAngle = Rotations.of(0);
+        Angle armTargetAngle = Rotations.of(0);
+        Current armMotorCurrent = Amps.of(0);
+
+        // Angle shooterPitch = Rotations.of(0);
+        // Angle shooterTargetPitch = Rotations.of(0);
+        // Current pitchMotorCurrent = Amps.of(0);
+
+        AngularVelocity rollerVelocity = RotationsPerSecond.of(0);
+        AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0);
+        Current rollerMotorCurrent = Amps.of(0);
+
+        // LinearVelocity feederVelocity = InchesPerSecond.of(0);
+        // LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
+        // Current feederMotorCurrent = Amps.of(0);
+    }
+
+    // public default void setShooterAngle(ShooterState state, Angle angle) {}
+    // public default void setShooterPitch(ShooterState state, Angle angle) {}
+    public default void setArmAngle(IntakeState state, Angle angle) {}
+    public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {}
+
+    public default void updateInputs(IntakeState state) {}
+}

src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java

@@ -0,0 +1,88 @@
+package frc4388.robot.subsystems.intake;
+
+import static edu.wpi.first.units.Units.InchesPerSecond;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.units.measure.*;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.utility.configurable.ConfigurableDouble;
+
+public class IntakeReal implements IntakeIO {
+
+
+    TalonFX m_armMotor;
+    TalonFX m_rollerMotor;
+
+    public IntakeReal(
+
+        TalonFX armMotor,
+        TalonFX rollerMotor
+    ) {
+        // m_angleMotor = angleMotor;
+        // m_pitchMotor = pitchMotor;
+        m_armMotor = armMotor;
+        m_rollerMotor = rollerMotor;
+
+        // Apply the configs
+        m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID);
+        m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG);
+        m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID);
+        m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG);
+    }
+
+    private Angle clampAng(Angle x, Angle min, Angle max){
+        if(x.gt(max)) {
+            return max;
+        }else if(x.lt(min)) {
+            return min;
+        }else{
+            return x;
+        }
+    }
+
+
+    
+    @Override
+    public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {
+        state.rollerTargetVelocity = angularVelocity;
+        // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC)
+        double motorSpeed = angularVelocity.in(RotationsPerSecond) / IntakeConstants.ROLLER_MOTOR_GEAR_RATIO;
+        VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
+        m_rollerMotor.setControl(velocity);
+    }
+
+    @Override
+    public void setArmAngle(IntakeState state, Angle angle) {
+        state.armTargetAngle = angle;
+        // Assume that the angle is always accurate, because I think we will use a shaft encoder
+        // Assume that 0 degrees = forwards. Might need an offset here
+        
+        Angle boundedAngle = clampAng(angle, IntakeConstants.ARM_LIMIT_LOWER, IntakeConstants.ARM_LIMIT_UPPER);
+        // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
+        double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.ARM_MOTOR_GEAR_RATIO;
+        PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
+        m_armMotor.setControl(posRequest);
+    }
+
+    @Override
+    public void updateInputs(IntakeState state) {
+        state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
+        state.armMotorCurrent = m_armMotor.getStatorCurrent(false).getValue();
+
+        // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
+        // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
+
+        // state.armAngle = m_armMotor.getPosition().getValue();
+        // state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue();
+
+        state.rollerVelocity = m_rollerMotor.getVelocity().getValue();
+        state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();
+    }
+}