fully automated intake

Signed-off-by: Jade Turner <[email protected]>

src/main/java/org/curtinfrc/frc2025/BuildConstants.java

@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2025-Reefscape";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 17;
-  public static final String GIT_SHA = "be9b4b89be864857eff3c275a4f3f4c9f4d7727c";
-  public static final String GIT_DATE = "2025-01-22 14:40:25 AWST";
+  public static final int GIT_REVISION = 42;
+  public static final String GIT_SHA = "2e09a1eba2cc71b41eddffee8ec070ceb4412e4c";
+  public static final String GIT_DATE = "2025-01-24 22:49:02 AWST";
   public static final String GIT_BRANCH = "Toby/intake";
-  public static final String BUILD_DATE = "2025-01-24 12:05:48 AWST";
-  public static final long BUILD_UNIX_TIME = 1737691548380L;
+  public static final String BUILD_DATE = "2025-01-24 23:28:41 AWST";
+  public static final long BUILD_UNIX_TIME = 1737732521172L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

src/main/java/org/curtinfrc/frc2025/Robot.java

@@ -1,6 +1,7 @@
 package org.curtinfrc.frc2025;
 
 import static org.curtinfrc.frc2025.subsystems.vision.VisionConstants.*;
+import static org.curtinfrc.subsystems.intake.IntakeConstants.intakeVolts;
 
 import choreo.auto.AutoFactory;
 import com.ctre.phoenix6.SignalLogger;
@@ -55,9 +56,9 @@
  */
 public class Robot extends LoggedRobot {
   // Subsystems
-  private final Drive drive;
-  private final Vision vision;
-  private final Intake intake;
+  private Drive drive;
+  private Vision vision;
+  private Intake intake;
   private Elevator elevator;
   private Superstructure superstructure;
 
@@ -259,6 +260,10 @@ public Robot() {
             () -> controller.getLeftX(),
             () -> -controller.getRightX()));
 
+    intake.setDefaultCommand(intake.intake(intakeVolts / 4));
+    intake.frontSensor.whileTrue(intake.intake(intakeVolts).until(intake.backSensor));
+    intake.backSensor.whileTrue(intake.intake(intakeVolts).until(intake.backSensor.negate()));
+
     // Lock to 0° when A button is held
     controller
         .a()

src/main/java/org/curtinfrc/subsystems/intake/Intake.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import org.littletonrobotics.junction.Logger;
 
 public class Intake extends SubsystemBase {
@@ -10,28 +11,22 @@ public class Intake extends SubsystemBase {
 
   public Intake(IntakeIO io) {
     this.io = io;
-    // setDefaultCommand(run(() -> io.setIntakeVolts(0)));
   }
 
+  public final Trigger frontSensor = new Trigger(() -> inputs.frontSensor);
+  public final Trigger backSensor = new Trigger(() -> inputs.backSensor);
+
   @Override
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("Intake", inputs);
   }
 
-  public void setIntakeVolts(double volts) {
-    io.setIntakeVolts(volts);
-  }
-
   public Command stop() {
-    return run(() -> io.setIntakeVolts(0));
-  }
-
-  public Command intakeCommand() {
-    return run(() -> io.setIntakeVolts(6));
+    return runOnce(() -> io.setVoltage(0));
   }
 
-  public Command goToTargetRPM() {
-    return run(() -> io.achieveRPM()).until(() -> io.intakeAtRPM());
+  public Command intake(double volts) {
+    return run(() -> io.setVoltage(volts));
   }
 }

src/main/java/org/curtinfrc/subsystems/intake/IntakeConstants.java

@@ -1,22 +1,10 @@
 package org.curtinfrc.subsystems.intake;
 
 public class IntakeConstants {
-  public static double intakeVolts = 6;
+  public static double intakeVolts = 8;
   public static int intakeCurrentLimit = 40;
   public static double motorReduction = 1.0;
-  public static double intakePort = 99;
-  public static double intakeMaxVelocity = 550;
-  public static double intakeTolerance = 0.01;
-  // public static double goalRPM = 4000;
-  public static double goalRPM = 500;
-  public static double intakeLoopError = 0.1;
-
-  public static double kP = 1;
-  public static double kI = 0;
-  public static double kD = 0;
-  public static double kV = 473;
-
-  public static double intakeID = 99;
-
-  public enum intakeSetPoints {}
+  public static int intakeMotorId = 99;
+  public static int intakeFrontSensorPort = 99;
+  public static int intakeBackSensorPort = 99;
 }

src/main/java/org/curtinfrc/subsystems/intake/IntakeIO.java

@@ -7,26 +7,13 @@ public interface IntakeIO {
   public static class IntakeIOInputs {
     public double appliedVolts;
     public double currentAmps;
-    // public AngularVelocity encoderOutput;
-    public double encoderOutput;
-    public double goalRPM = 500;
-    public boolean intakeAtRPM = false;
-    // double intakeVolts = IntakeConstants.intakeVolts;
+    public double positionRotations;
+    public double angularVelocityRotationsPerMinute;
+    public boolean frontSensor;
+    public boolean backSensor;
   }
 
   public default void updateInputs(IntakeIOInputs inputs) {}
 
-  public default void achieveRPM() {}
-
-  public default boolean intakeAtRPM() {
-    return false;
-  }
-
-  public default void beamBreakState() {}
-
-  public default void setIntakeVolts(double volts) {}
-
-  public default void intake() {}
-
-  public default void stop() {}
+  public default void setVoltage(double volts) {}
 }

src/main/java/org/curtinfrc/subsystems/intake/IntakeIONEO.java

@@ -1,19 +1,31 @@
 package org.curtinfrc.subsystems.intake;
 
-import com.revrobotics.RelativeEncoder;
+import static org.curtinfrc.subsystems.intake.IntakeConstants.*;
+
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
+import edu.wpi.first.wpilibj.DigitalInput;
 
 public class IntakeIONEO implements IntakeIO {
-  protected final SparkMax intakeNeo = new SparkMax(99, MotorType.kBrushless);
-  protected final RelativeEncoder intakeEncoder = intakeNeo.getEncoder();
+  private final SparkMax intakeNeo = new SparkMax(intakeMotorId, MotorType.kBrushless);
+  private final DigitalInput frontSensor = new DigitalInput(intakeFrontSensorPort);
+  private final DigitalInput backSensor = new DigitalInput(intakeBackSensorPort);
 
+  // TODO apply configs
   public IntakeIONEO() {}
 
   @Override
   public void updateInputs(IntakeIOInputs inputs) {
-    inputs.encoderOutput = intakeEncoder.getVelocity();
-    // inputs.encoderOutput = intakeEncoder.getVelocity();
     inputs.appliedVolts = intakeNeo.getBusVoltage() * intakeNeo.getAppliedOutput();
+    inputs.currentAmps = intakeNeo.getOutputCurrent();
+    inputs.positionRotations = intakeNeo.getEncoder().getPosition();
+    inputs.angularVelocityRotationsPerMinute = intakeNeo.getEncoder().getVelocity();
+    inputs.frontSensor = frontSensor.get();
+    inputs.backSensor = backSensor.get();
+  }
+
+  @Override
+  public void setVoltage(double volts) {
+    intakeNeo.setVoltage(volts);
   }
 }

src/main/java/org/curtinfrc/subsystems/intake/IntakeIOSim.java

@@ -1,47 +1,49 @@
 package org.curtinfrc.subsystems.intake;
 
-import edu.wpi.first.math.MathUtil;
+import static org.curtinfrc.subsystems.intake.IntakeConstants.intakeBackSensorPort;
+import static org.curtinfrc.subsystems.intake.IntakeConstants.intakeFrontSensorPort;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 
 public class IntakeIOSim implements IntakeIO {
-
   private DCMotor intakeMotor = DCMotor.getNEO(1);
   private DCMotorSim intakeMotorSim;
+  private SimDevice frontImpl;
+  private SimBoolean frontSensor;
+  private SimDevice backImpl;
+  private SimBoolean backSensor;
+  private double volts = 0;
 
   public IntakeIOSim() {
     intakeMotorSim =
         new DCMotorSim(LinearSystemId.createDCMotorSystem(intakeMotor, 0.025, 4.0), intakeMotor);
-  }
 
-  private double appliedVolts = 0.0;
+    frontImpl = SimDevice.create("IntakeSensorFront", intakeFrontSensorPort);
+    frontSensor = frontImpl.createBoolean("IsTriggered", Direction.kInput, false);
+    backImpl = SimDevice.create("IntakeSensorBack", intakeBackSensorPort);
+    backSensor = backImpl.createBoolean("IsTriggered", Direction.kInput, false);
+  }
 
   @Override
   public void updateInputs(IntakeIOInputs inputs) {
-    intakeMotorSim.setInputVoltage(appliedVolts);
+    intakeMotorSim.setInputVoltage(volts);
     intakeMotorSim.update(0.02);
-    // inputs.appliedVolts = appliedVolts;
     inputs.appliedVolts = intakeMotorSim.getInputVoltage();
-    // inputs.encoderOutput = intakeMotorSim.getAngularVelocity();
-    inputs.encoderOutput = intakeMotorSim.getAngularVelocityRadPerSec();
+    inputs.currentAmps = intakeMotorSim.getCurrentDrawAmps();
+    inputs.positionRotations = intakeMotorSim.getAngularPositionRotations();
+    inputs.angularVelocityRotationsPerMinute = intakeMotorSim.getAngularVelocityRPM();
+    inputs.frontSensor = frontSensor.get();
+    inputs.backSensor = backSensor.get();
   }
 
   @Override
-  public void setIntakeVolts(double volts) {
-    appliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
+  public void setVoltage(double volts) {
+    this.volts = volts;
     intakeMotorSim.setInputVoltage(volts);
   }
-
-  @Override
-  public void achieveRPM() {
-    intakeMotorSim.setAngularVelocity(IntakeConstants.goalRPM);
-  }
-
-  @Override
-  public boolean intakeAtRPM() {
-    double angularVelocity = intakeMotorSim.getAngularVelocityRadPerSec();
-    return IntakeConstants.goalRPM - IntakeConstants.intakeTolerance < angularVelocity
-        && angularVelocity < IntakeConstants.goalRPM + IntakeConstants.intakeTolerance;
-  }
 }