Add ejector code

Signed-off-by: Jade Turner <[email protected]>
Co-authored-by: JoystickMaster-race <[email protected]>

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

@@ -24,6 +24,10 @@
 import org.curtinfrc.frc2025.subsystems.drive.ModuleIO;
 import org.curtinfrc.frc2025.subsystems.drive.ModuleIOSim;
 import org.curtinfrc.frc2025.subsystems.drive.ModuleIOTalonFX;
+import org.curtinfrc.frc2025.subsystems.ejector.Ejector;
+import org.curtinfrc.frc2025.subsystems.ejector.EjectorIO;
+import org.curtinfrc.frc2025.subsystems.ejector.EjectorIONEO;
+import org.curtinfrc.frc2025.subsystems.ejector.EjectorIOSim;
 import org.curtinfrc.frc2025.subsystems.elevator.Elevator;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIO;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIONeoMaxMotionLaserCAN;
@@ -60,6 +64,7 @@ public class Robot extends LoggedRobot {
   private Vision vision;
   private Intake intake;
   private Elevator elevator;
+  private Ejector ejector;
   private Superstructure superstructure;
 
   // Controller
@@ -138,6 +143,7 @@ public Robot() {
           // elevator = new Elevator(new ElevatorIONeoMaxMotionLaserCAN());
           elevator = new Elevator(new ElevatorIO() {});
           intake = new Intake(new IntakeIONEO());
+          ejector = new Ejector(new EjectorIONEO());
         }
 
         case DEVBOT -> {
@@ -157,6 +163,7 @@ public Robot() {
                   new VisionIOQuestNav());
           elevator = new Elevator(new ElevatorIONeoMaxMotionLaserCAN());
           intake = new Intake(new IntakeIONEO());
+          ejector = new Ejector(new EjectorIONEO());
         }
 
         case SIMBOT -> {
@@ -179,6 +186,7 @@ public Robot() {
 
           elevator = new Elevator(new ElevatorIOSim());
           intake = new Intake(new IntakeIOSim());
+          ejector = new Ejector(new EjectorIOSim());
         }
       }
     } else {
@@ -196,6 +204,7 @@ public Robot() {
 
       elevator = new Elevator(new ElevatorIO() {});
       intake = new Intake(new IntakeIO() {});
+      ejector = new Ejector(new EjectorIO() {});
     }
 
     superstructure = new Superstructure(drive, elevator);
@@ -261,8 +270,12 @@ public Robot() {
             () -> -controller.getRightX()));
 
     intake.setDefaultCommand(intake.intake(intakeVolts / 4));
+    ejector.setDefaultCommand(ejector.stop());
     intake.frontSensor.whileTrue(intake.intake(intakeVolts).until(intake.backSensor));
     intake.backSensor.whileTrue(intake.intake(intakeVolts).until(intake.backSensor.negate()));
+    intake.backSensor.whileTrue(ejector.eject(15).until(ejector.sensor));
+
+    controller.x().whileTrue(ejector.eject(1500).until(ejector.sensor.negate()));
 
     // Lock to 0° when A button is held
     controller

src/main/java/org/curtinfrc/frc2025/subsystems/ejector/Ejector.java

@@ -0,0 +1,45 @@
+package org.curtinfrc.frc2025.subsystems.ejector;
+
+import static org.curtinfrc.frc2025.subsystems.ejector.EjectorConstants.*;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+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 Ejector extends SubsystemBase {
+  private final EjectorIO io;
+  private final EjectorIOInputsAutoLogged inputs = new EjectorIOInputsAutoLogged();
+  private final PIDController pid = new PIDController(kP, 0, kD);
+  private final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kS, kV, kA);
+
+  public Ejector(EjectorIO io) {
+    this.io = io;
+  }
+
+  public final Trigger sensor = new Trigger(() -> inputs.sensor);
+  public final Trigger atSetpoint = new Trigger(pid::atSetpoint);
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("Ejector", inputs);
+  }
+
+  public Command stop() {
+    return runOnce(() -> io.setVoltage(0));
+  }
+
+  public Command eject(double rpm) {
+    return run(
+        () -> {
+          Logger.recordOutput("Ejector/VelocitySetpoint", rpm);
+          var pid_out = pid.calculate(inputs.angularVelocityRotationsPerMinute, rpm);
+          Logger.recordOutput("Ejector/VelocityErrror", pid.getError());
+          var ff_out = ff.calculate(rpm);
+          io.setVoltage(pid_out + ff_out);
+        });
+  }
+}

src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorConstants.java

@@ -0,0 +1,14 @@
+package org.curtinfrc.frc2025.subsystems.ejector;
+
+public class EjectorConstants {
+  public static final int motorId = 99;
+  public static final int sensorPort = 99;
+  public static final int currentLimit = 60;
+  public static final double ejectorMoi = 0.025;
+  public static final double ejectorReduction = 1;
+  public static final double kP = 1.0;
+  public static final double kD = 0;
+  public static final double kS = 0;
+  public static final double kV = 473;
+  public static final double kA = 0;
+}

src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorIO.java

@@ -0,0 +1,18 @@
+package org.curtinfrc.frc2025.subsystems.ejector;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface EjectorIO {
+  @AutoLog
+  public static class EjectorIOInputs {
+    public double appliedVolts;
+    public double currentAmps;
+    public double positionRotations;
+    public double angularVelocityRotationsPerMinute;
+    public boolean sensor;
+  }
+
+  public default void updateInputs(EjectorIOInputs inputs) {}
+
+  public default void setVoltage(double volts) {}
+}

src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorIONEO.java

@@ -0,0 +1,33 @@
+package org.curtinfrc.frc2025.subsystems.ejector;
+
+import static org.curtinfrc.frc2025.subsystems.ejector.EjectorConstants.*;
+
+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 edu.wpi.first.wpilibj.DigitalInput;
+
+public class EjectorIONEO implements EjectorIO {
+  private final SparkMax intakeNeo = new SparkMax(motorId, MotorType.kBrushless);
+  private final DigitalInput sensor = new DigitalInput(sensorPort);
+
+  public EjectorIONEO() {
+    SparkMaxConfig config = new SparkMaxConfig();
+    config.smartCurrentLimit(0, currentLimit).idleMode(IdleMode.kCoast);
+  }
+
+  @Override
+  public void updateInputs(EjectorIOInputs inputs) {
+    inputs.appliedVolts = intakeNeo.getBusVoltage() * intakeNeo.getAppliedOutput();
+    inputs.currentAmps = intakeNeo.getOutputCurrent();
+    inputs.positionRotations = intakeNeo.getEncoder().getPosition();
+    inputs.angularVelocityRotationsPerMinute = intakeNeo.getEncoder().getVelocity();
+    inputs.sensor = sensor.get();
+  }
+
+  @Override
+  public void setVoltage(double volts) {
+    intakeNeo.setVoltage(volts);
+  }
+}

src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorIOSim.java

@@ -0,0 +1,45 @@
+package org.curtinfrc.frc2025.subsystems.ejector;
+
+import static org.curtinfrc.frc2025.subsystems.ejector.EjectorConstants.*;
+
+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 EjectorIOSim implements EjectorIO {
+  private DCMotor intakeMotor = DCMotor.getNEO(1);
+  private DCMotorSim intakeMotorSim;
+  private SimDevice sensorImpl;
+  private SimBoolean sensor;
+  private double volts = 0;
+
+  public EjectorIOSim() {
+    intakeMotorSim =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(intakeMotor, ejectorMoi, ejectorReduction),
+            intakeMotor);
+
+    sensorImpl = SimDevice.create("EjectorSensor", sensorPort);
+    sensor = sensorImpl.createBoolean("IsTriggered", Direction.kInput, false);
+  }
+
+  @Override
+  public void updateInputs(EjectorIOInputs inputs) {
+    intakeMotorSim.setInputVoltage(volts);
+    intakeMotorSim.update(0.02);
+    inputs.appliedVolts = intakeMotorSim.getInputVoltage();
+    inputs.currentAmps = intakeMotorSim.getCurrentDrawAmps();
+    inputs.positionRotations = intakeMotorSim.getAngularPositionRotations();
+    inputs.angularVelocityRotationsPerMinute = intakeMotorSim.getAngularVelocityRPM();
+    inputs.sensor = sensor.get();
+  }
+
+  @Override
+  public void setVoltage(double volts) {
+    this.volts = volts;
+    intakeMotorSim.setInputVoltage(volts);
+  }
+}