Initial Coral Subsystem

src/main/java/frc/robot/constants/CoralConstants.java

@@ -1,3 +1,80 @@
 package frc.robot.constants;
 
-public class CoralConstants {}
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
+import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
+import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
+import edu.wpi.first.units.measure.AngularVelocity;
+
+public class CoralConstants {
+  public static int kCoralFxId = 0;
+
+  public static final AngularVelocity kCloseEnough = DegreesPerSecond.of(5);
+
+  // Coral Talon FX Config
+  public static TalonFXConfiguration getFXConfig() {
+    TalonFXConfiguration fxConfig = new TalonFXConfiguration();
+
+    CurrentLimitsConfigs current =
+        new CurrentLimitsConfigs()
+            .withStatorCurrentLimit(10)
+            .withStatorCurrentLimitEnable(false)
+            .withStatorCurrentLimit(20)
+            .withSupplyCurrentLimit(10)
+            .withSupplyCurrentLowerLimit(8)
+            .withSupplyCurrentLowerTime(0.02)
+            .withSupplyCurrentLimitEnable(true);
+    fxConfig.CurrentLimits = current;
+
+    HardwareLimitSwitchConfigs hwLimit =
+        new HardwareLimitSwitchConfigs()
+            .withForwardLimitAutosetPositionEnable(false)
+            .withForwardLimitEnable(false)
+            .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
+            .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
+            .withReverseLimitAutosetPositionEnable(false)
+            .withReverseLimitEnable(false)
+            .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
+            .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
+    fxConfig.HardwareLimitSwitch = hwLimit;
+
+    Slot0Configs slot0 =
+        new Slot0Configs()
+            .withKP(0)
+            .withKI(0)
+            .withKD(0)
+            .withGravityType(GravityTypeValue.Elevator_Static)
+            .withKG(0)
+            .withKS(0)
+            .withKV(0)
+            .withKA(0);
+    fxConfig.Slot0 = slot0;
+
+    MotionMagicConfigs motionMagic =
+        new MotionMagicConfigs()
+            .withMotionMagicAcceleration(0)
+            .withMotionMagicCruiseVelocity(0)
+            .withMotionMagicExpo_kA(0)
+            .withMotionMagicExpo_kV(0)
+            .withMotionMagicJerk(0);
+    fxConfig.MotionMagic = motionMagic;
+
+    MotorOutputConfigs motorOut =
+        new MotorOutputConfigs()
+            .withDutyCycleNeutralDeadband(0.01)
+            .withNeutralMode(NeutralModeValue.Coast);
+    fxConfig.MotorOutput = motorOut;
+
+    return fxConfig;
+  }
+}

src/main/java/frc/robot/subsystems/coral/CoralIO.java

@@ -1,3 +1,27 @@
 package frc.robot.subsystems.coral;
 
-public class CoralIO {}
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import edu.wpi.first.units.measure.AngularVelocity;
+import org.littletonrobotics.junction.AutoLog;
+import org.strykeforce.telemetry.TelemetryService;
+
+public interface CoralIO {
+
+  @AutoLog
+  public static class CoralIOInputs {
+    public AngularVelocity velocity = RotationsPerSecond.of(0.0);
+    public boolean isFwdLimitSwitchClosed = false;
+    public boolean isRevLimitSwitchClosed = false;
+  }
+
+  public default void setVelocity(AngularVelocity velocity) {}
+
+  public default void enableFwdLimitSwitch(boolean enabled) {}
+
+  public default void enableRevLimitSwitch(boolean enabled) {}
+
+  public default void updateInputs(CoralIOInputs inputs) {}
+
+  public default void registerWith(TelemetryService telemetryService) {}
+}

src/main/java/frc/robot/subsystems/coral/CoralIOFX.java

@@ -0,0 +1,78 @@
+package frc.robot.subsystems.coral;
+
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.ForwardLimitValue;
+import com.ctre.phoenix6.signals.ReverseLimitValue;
+import edu.wpi.first.units.measure.AngularVelocity;
+import frc.robot.constants.CoralConstants;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
+
+public class CoralIOFX implements CoralIO {
+  // private objects
+  private Logger logger;
+  private TalonFX talonFx;
+
+  // FX Access objects
+  TalonFXConfigurator configurator;
+  private MotionMagicVelocityDutyCycle velocityRequest =
+      new MotionMagicVelocityDutyCycle(0).withEnableFOC(false).withSlot(0);
+  StatusSignal<AngularVelocity> curVelocity;
+  StatusSignal<ForwardLimitValue> fwdLimitSwitch;
+  StatusSignal<ReverseLimitValue> revLimitSwitch;
+
+  public CoralIOFX() {
+    logger = LoggerFactory.getLogger(this.getClass());
+    talonFx = new TalonFX(CoralConstants.kCoralFxId);
+
+    // Config controller
+    configurator = talonFx.getConfigurator();
+    configurator.apply(new TalonFXConfiguration()); // Factory default motor controller
+    configurator.apply(CoralConstants.getFXConfig());
+
+    // Attach status signals
+    curVelocity = talonFx.getVelocity();
+    fwdLimitSwitch = talonFx.getForwardLimit();
+    revLimitSwitch = talonFx.getReverseLimit();
+  }
+
+  @Override
+  public void setVelocity(AngularVelocity velocity) {
+    logger.info("Setting velocity to {} degrees per second", velocity.in(DegreesPerSecond));
+
+    talonFx.setControl(velocityRequest.withVelocity(velocity));
+  }
+
+  @Override
+  public void enableFwdLimitSwitch(boolean enabled) {
+    talonFx
+        .getConfigurator()
+        .apply(CoralConstants.getFXConfig().HardwareLimitSwitch.withForwardLimitEnable(enabled));
+  }
+
+  @Override
+  public void enableRevLimitSwitch(boolean enabled) {
+    talonFx
+        .getConfigurator()
+        .apply(CoralConstants.getFXConfig().HardwareLimitSwitch.withReverseLimitEnable(enabled));
+  }
+
+  @Override
+  public void updateInputs(CoralIOInputs inputs) {
+    inputs.velocity = curVelocity.refresh().getValue();
+    inputs.isFwdLimitSwitchClosed = fwdLimitSwitch.refresh().getValue().value == 1;
+    inputs.isRevLimitSwitchClosed = revLimitSwitch.refresh().getValue().value == 0;
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    telemetryService.register(talonFx, true);
+  }
+}

src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java

@@ -1,3 +1,85 @@
 package frc.robot.subsystems.coral;
 
-public class CoralSubsystem {}
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import edu.wpi.first.units.measure.AngularVelocity;
+import frc.robot.constants.CoralConstants;
+import frc.robot.standards.ClosedLoopSpeedSubsystem;
+import java.util.Set;
+import org.littletonrobotics.junction.Logger;
+import org.strykeforce.telemetry.TelemetryService;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
+
+public class CoralSubsystem extends MeasurableSubsystem implements ClosedLoopSpeedSubsystem {
+  private final CoralIO io;
+  private final CoralIOInputsAutoLogged inputs = new CoralIOInputsAutoLogged();
+  private AngularVelocity setpoint = RotationsPerSecond.of(0.0);
+  private CoralState curState = CoralState.INIT;
+
+  public CoralSubsystem(CoralIO io) {
+    this.io = io;
+  }
+
+  public CoralState getState() {
+    return curState;
+  }
+
+  @Override
+  public AngularVelocity getSpeed() {
+    return inputs.velocity;
+  }
+
+  @Override
+  public void setSpeed(AngularVelocity speed) {
+    setpoint = speed;
+    io.setVelocity(speed);
+  }
+
+  @Override
+  public boolean atSpeed() {
+    return setpoint.minus(inputs.velocity).abs(RotationsPerSecond)
+        <= CoralConstants.kCloseEnough.in(RotationsPerSecond);
+  }
+
+  public boolean isBeamBroken() {
+    return false; // FIXME when we get the robot
+  }
+
+  // Periodic Function
+  @Override
+  public void periodic() {
+    // Read Inputs
+    io.updateInputs(inputs);
+
+    // State Machine
+    switch (curState) {
+      case INIT:
+        break;
+      case ZEROED:
+        break;
+      default:
+        break;
+    }
+
+    // Log Outputs
+    Logger.recordOutput("Coral/curState", curState.ordinal());
+    Logger.recordOutput("Coral/setpoint", setpoint.in(RotationsPerSecond));
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    super.registerWith(telemetryService);
+    io.registerWith(telemetryService);
+  }
+
+  @Override
+  public Set<Measure> getMeasures() {
+    return Set.of(new Measure("State", () -> curState.ordinal()));
+  }
+
+  public enum CoralState {
+    INIT,
+    ZEROED
+  }
+}