Feature/intake

.github/prompts/code-review-prompt.md

@@ -222,7 +222,7 @@ public abstract class ServoMotorSubsystem<IO extends MotorIO> extends SubsystemB
 }
 ```
 
-12. Safety & Error Handling
+## Safety & Error Handling
 
 ### Mechanism Limits
 

.github/scripts/gemini-code-review.py

@@ -12,7 +12,7 @@
 from github import Github, GithubException
 
 # Configuration
-GEMINI_MODEL = "gemini-2.0-flash-exp"
+GEMINI_MODEL = "gemini-2.5-flash"
 MAX_FILES_TO_REVIEW = 50
 MAX_FILE_SIZE = 100000  # 100KB per file
 REVIEW_COMMENT_MARKER = "<!-- ai-code-review-bot -->"

.github/workflows/ai-code-review.yml

@@ -19,8 +19,9 @@ on:
         default: false
 
 permissions:
-  contents: read
-  pull-requests: write
+  issues: write      # Allows reading and posting comments to issues
+  pull-requests: write # Allows reading and posting to PRs
+  contents: read     # Standard permission to read the code
 
 jobs:
   ai-review:

src/main/java/frc/robot/preferences/BooleanPreference.java

@@ -0,0 +1,27 @@
+package frc.robot.preferences;
+
+import edu.wpi.first.wpilibj.Preferences;
+
+public class BooleanPreference extends RobotPreference<Boolean> {
+  private boolean defaultValue;
+
+  public BooleanPreference(String key) {
+    this(key, false);
+  }
+
+  public BooleanPreference(String key, boolean defaultValue) {
+    super(key);
+    this.defaultValue = defaultValue;
+
+    Preferences.initBoolean(key, defaultValue);
+  }
+
+  public boolean getValue() {
+    return Preferences.getBoolean(super.key, defaultValue);
+  }
+
+  @Override
+  public Boolean get() {
+    return getValue();
+  }
+}

src/main/java/frc/robot/preferences/DoublePreference.java

@@ -0,0 +1,27 @@
+package frc.robot.preferences;
+
+import edu.wpi.first.wpilibj.Preferences;
+
+public class DoublePreference extends RobotPreference<Double> {
+  private double defaultValue;
+
+  public DoublePreference(String key) {
+    this(key, 0.0);
+  }
+
+  public DoublePreference(String key, double defaultValue) {
+    super(key);
+    this.defaultValue = defaultValue;
+
+    Preferences.initDouble(key, defaultValue);
+  }
+
+  public double getValue() {
+    return Preferences.getDouble(super.key, defaultValue);
+  }
+
+  @Override
+  public Double get() {
+    return getValue();
+  }
+}

src/main/java/frc/robot/preferences/IntegerPreference.java

@@ -0,0 +1,27 @@
+package frc.robot.preferences;
+
+import edu.wpi.first.wpilibj.Preferences;
+
+public class IntegerPreference extends RobotPreference<Integer> {
+  private int defaultValue;
+
+  public IntegerPreference(String key) {
+    this(key, 0);
+  }
+
+  public IntegerPreference(String key, int defaultValue) {
+    super(key);
+    this.defaultValue = defaultValue;
+
+    Preferences.initInt(key, defaultValue);
+  }
+
+  public int getValue() {
+    return Preferences.getInt(super.key, defaultValue);
+  }
+
+  @Override
+  public Integer get() {
+    return getValue();
+  }
+}

src/main/java/frc/robot/preferences/RobotPreference.java

@@ -0,0 +1,25 @@
+package frc.robot.preferences;
+
+import edu.wpi.first.wpilibj.Preferences;
+import java.util.Collection;
+import java.util.function.Supplier;
+
+public abstract class RobotPreference<T> implements Supplier<T> {
+  protected String key;
+
+  public RobotPreference(String key) {
+    this.key = key;
+  }
+
+  public void remove() {
+    Preferences.remove(key);
+  }
+
+  public static void removeAll() {
+    Preferences.removeAll();
+  }
+
+  public static Collection<String> getKeys() {
+    return Preferences.getKeys();
+  }
+}

src/main/java/frc/robot/preferences/StringPreference.java

@@ -0,0 +1,27 @@
+package frc.robot.preferences;
+
+import edu.wpi.first.wpilibj.Preferences;
+
+public class StringPreference extends RobotPreference<String> {
+  private String defaultValue;
+
+  public StringPreference(String key) {
+    this(key, "");
+  }
+
+  public StringPreference(String key, String defaultValue) {
+    super(key);
+    this.defaultValue = defaultValue;
+
+    Preferences.initString(key, defaultValue);
+  }
+
+  public String getValue() {
+    return Preferences.getString(super.key, defaultValue);
+  }
+
+  @Override
+  public String get() {
+    return getValue();
+  }
+}

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

@@ -0,0 +1,58 @@
+package frc.robot.subsystems.intake;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
+
+public class IntakeConstants {
+
+  private IntakeConstants() {}
+
+  // TODO: Change to actual ports
+  public static final int ROLLER_MOTOR_ID = 2;
+  public static final int EXTENSION_MOTOR_ID = 3;
+
+  // TODO: Tune Roller and Extension Motors
+
+  // Roller Motor
+  public static final double ROLLER_KS = 0;
+  public static final double ROLLER_KV = 0;
+  public static final double ROLLER_KP = 0;
+  public static final double ROLLER_KD = 0;
+
+  public static Slot0Configs createRollerMotorSlot0Configs() {
+    Slot0Configs slot = new Slot0Configs();
+    slot.kS = ROLLER_KS;
+    slot.kV = ROLLER_KV;
+    slot.kP = ROLLER_KP;
+    slot.kD = ROLLER_KD;
+    return slot;
+  }
+
+  // Extension Motor
+  public static final double ALLOWABLE_EXTENSION_ERROR = 0.1;
+  public static final double INTAKE_FORWARD_LIMIT = 100;
+  public static final double INTAKE_REVERSE_LIMIT = 0;
+  public static final double EXTENSION_KS = 0;
+  public static final double EXTENSION_KP = 0;
+  public static final double EXTENSION_KD = 0;
+
+  public static Slot0Configs createExtensionMotorSlot0Configs() {
+    Slot0Configs slot = new Slot0Configs();
+    slot.kS = EXTENSION_KS;
+    slot.kP = EXTENSION_KP;
+    slot.kD = EXTENSION_KD;
+    return slot;
+  }
+
+  public static SoftwareLimitSwitchConfigs createExtensionSoftwareLimitSwitchConfigs() {
+    SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs();
+    configs.ForwardSoftLimitEnable = false;
+    configs.ReverseSoftLimitEnable = false;
+    configs.ForwardSoftLimitThreshold = INTAKE_FORWARD_LIMIT;
+    configs.ReverseSoftLimitThreshold = INTAKE_REVERSE_LIMIT;
+    return configs;
+  }
+
+  public static final double SAFE_HOMING_EFFORT = -0.2;
+  public static final double SAFE_STATOR_LIMIT = 0.8;
+}

src/main/java/frc/robot/subsystems/intake/IntakePreferences.java

@@ -0,0 +1,14 @@
+package frc.robot.subsystems.intake;
+
+import frc.robot.preferences.DoublePreference;
+
+public class IntakePreferences {
+
+  private IntakePreferences() {}
+  ;
+
+  public static DoublePreference rollerIntakeSpeed =
+      new DoublePreference("Intake/Roller Intake Speed", 0.1); // in rotations per second
+  public static DoublePreference intakeCollectPosition =
+      new DoublePreference("Intake/Stowed Intake Position", 3); // in rotations
+}

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -0,0 +1,94 @@
+package frc.robot.subsystems.intake;
+
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class IntakeSubsystem extends SubsystemBase {
+  private final TalonFX rollerMotor;
+  private final TalonFX extensionMotor;
+
+  private AngularVelocity rollerVelocityTarget; // RotationsPerSecond
+  private VelocityVoltage rollerControl;
+
+  private Angle extensionTarget; // Rotations
+  private PositionTorqueCurrentFOC extensionControl;
+
+  public IntakeSubsystem() {
+    rollerMotor = new TalonFX(IntakeConstants.ROLLER_MOTOR_ID);
+    extensionMotor = new TalonFX(IntakeConstants.EXTENSION_MOTOR_ID);
+
+    rollerMotor.getConfigurator().apply(IntakeConstants.createRollerMotorSlot0Configs());
+    rollerVelocityTarget = RotationsPerSecond.of(0);
+    rollerControl = new VelocityVoltage(0);
+
+    extensionMotor.getConfigurator().apply(IntakeConstants.createExtensionMotorSlot0Configs());
+    extensionMotor
+        .getConfigurator()
+        .apply(IntakeConstants.createExtensionSoftwareLimitSwitchConfigs());
+    extensionMotor.setPosition(0);
+    extensionTarget = Rotations.of(0);
+    extensionControl = new PositionTorqueCurrentFOC(0);
+  }
+
+  @Override
+  public void periodic() {
+    rollerMotor.setControl(rollerControl.withVelocity(rollerVelocityTarget.in(RotationsPerSecond)));
+    extensionMotor.setControl(extensionControl.withPosition(extensionTarget.in(Rotations)));
+  }
+
+  public Command spinRollerCommand() {
+    return runOnce(
+            () ->
+                rollerVelocityTarget =
+                    RotationsPerSecond.of(IntakePreferences.rollerIntakeSpeed.getValue()))
+        .withName("Spin Intake Roller");
+  }
+
+  public Command stopRollerCommand() {
+    return runOnce(() -> rollerVelocityTarget = RotationsPerSecond.of(0))
+        .withName("Stop Intake Roller");
+  }
+
+  private boolean atExtensionSetpoint() {
+    return Math.abs(extensionMotor.getPosition().getValueAsDouble() - extensionTarget.in(Rotations))
+        < IntakeConstants.ALLOWABLE_EXTENSION_ERROR;
+  }
+
+  public Command setIntakeExtensionCommand(Angle position) {
+    return runOnce(() -> extensionTarget = position)
+        .andThen(Commands.waitUntil(() -> atExtensionSetpoint()));
+  }
+
+  public Command collectCommand() {
+    return setIntakeExtensionCommand(
+            Rotations.of(IntakePreferences.intakeCollectPosition.getValue()))
+        .andThen(spinRollerCommand())
+        .withName("Activate Intake Collection");
+  }
+
+  public Command stowCommand() {
+    return stopRollerCommand()
+        .andThen(setIntakeExtensionCommand(Rotations.of(0)))
+        .withName("Stow Intake");
+  }
+
+  public Command homeIntakeCommand() {
+    return runEnd(
+            () -> extensionMotor.set(IntakeConstants.SAFE_HOMING_EFFORT),
+            () -> extensionMotor.setPosition(0))
+        .until(
+            () -> {
+              return extensionMotor.getStatorCurrent().getValueAsDouble()
+                  > IntakeConstants.SAFE_STATOR_LIMIT;
+            });
+  }
+}