Initial Biscuit Subsystem

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Main",
+      "request": "launch",
+      "mainClass": "frc.robot.Main",
+      "projectName": "reefscape"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

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

@@ -1,3 +1,103 @@
 package frc.robot.constants;
 
-public class BiscuitConstants {}
+import static edu.wpi.first.units.Units.Rotations;
+
+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.SoftwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+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.Angle;
+
+public class BiscuitConstants {
+  // These are all wrong right now because we don't have any actual info
+
+  public static Angle kZero = Rotations.of(42); // Will need to be experimentally determined
+  public static int talonID = 3;
+  public static double kCloseEnough = 2137473647; // This is a little out of wack.
+  public static final Angle kMaxFwd = Rotations.of(100);
+  public static final Angle kMaxRev = Rotations.of(-100);
+  // public static Angle Level1 = ;
+
+  // Disables the TalonFX by setting it's voltage to zero. Not very shocking.
+  public static VoltageConfigs disableTalon() {
+    VoltageConfigs voltage =
+        new VoltageConfigs().withPeakForwardVoltage(0.0).withPeakReverseVoltage(0.0);
+    getFXConfig().Voltage = voltage;
+    return voltage;
+  }
+
+  // I copied and pasted this because I'm lazy
+  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;
+
+    SoftwareLimitSwitchConfigs swLimit =
+        new SoftwareLimitSwitchConfigs()
+            .withForwardSoftLimitEnable(true)
+            .withForwardSoftLimitThreshold(kMaxFwd)
+            .withReverseSoftLimitEnable(true)
+            .withReverseSoftLimitThreshold(kMaxRev);
+    fxConfig.SoftwareLimitSwitch = swLimit;
+
+    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/biscuit/BiscuitIO.java

@@ -1,3 +1,28 @@
 package frc.robot.subsystems.biscuit;
 
-public class BiscuitIO {}
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import org.littletonrobotics.junction.AutoLog;
+import org.strykeforce.telemetry.TelemetryService;
+
+public interface BiscuitIO {
+
+  @AutoLog
+  public class BiscuitIOInputs {
+    public Angle position = Rotations.of(0);
+    public AngularVelocity velocity = RotationsPerSecond.of(0);
+    public boolean fwdLimitSwitchOpen = false;
+    public boolean didZero;
+  }
+
+  public default void setPosition(Angle position) {}
+
+  public default void updateInputs(BiscuitIOInputs inputs) {}
+
+  public default void registerWith(TelemetryService telemetry) {}
+
+  public default void zero() {}
+}

src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java

@@ -1,3 +1,83 @@
 package frc.robot.subsystems.biscuit;
 
-public class BiscuitIOFX {}
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import frc.robot.constants.BiscuitConstants;
+import frc.robot.subsystems.biscuit.BiscuitIO.BiscuitIOInputs;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
+
+public class BiscuitIOFX implements BiscuitIO {
+
+  private Logger logger;
+  private TalonFX talon;
+
+  private final Angle sensorInitial;
+  private StatusSignal<Angle> position;
+  private StatusSignal<AngularVelocity> velocity;
+  private StatusSignal<ForwardLimitTypeValue> fwdLimitSwitch;
+  private boolean didZero;
+  private boolean fwdLimitSwitchOpen;
+  private Angle offset;
+  private Alert rangeAlert = new Alert("Biscuit overextended! Shuting down!", AlertType.kError);
+
+  TalonFXConfigurator configurator;
+  private MotionMagicDutyCycle positionRequest =
+      new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0);
+
+  public BiscuitIOFX() {
+    // Logger initialization with class name
+    logger = LoggerFactory.getLogger(this.getClass());
+    // Moter initialization with ID from constants
+    talon = new TalonFX(BiscuitConstants.talonID);
+    // Set the starting encoder position
+    sensorInitial = talon.getPosition().getValue();
+
+    // Reset and configure moter settings
+    configurator = talon.getConfigurator();
+    configurator.apply(new TalonFXConfiguration());
+    configurator.apply(BiscuitConstants.getFXConfig());
+    // Set our variables
+    velocity = talon.getVelocity();
+    position = talon.getPosition();
+  }
+
+  public void setPosition(Angle position) {
+    talon.setControl(positionRequest.withPosition(position));
+  }
+
+  public void updateInputs(BiscuitIOInputs inputs) {
+    BaseStatusSignal.refreshAll(velocity, position, fwdLimitSwitch);
+    inputs.velocity = velocity.getValue();
+    inputs.position = position.getValue();
+    inputs.fwdLimitSwitchOpen = fwdLimitSwitch.getValueAsDouble() == 1;
+    inputs.didZero = didZero;
+  }
+
+  public void registerWith(TelemetryService telemetry) {
+    telemetry.register(talon, true);
+  }
+
+  public void zero() {
+    didZero = false;
+    if (fwdLimitSwitchOpen == true) {
+      Angle pos = position.getValue();
+      offset = BiscuitConstants.kZero.minus(pos);
+      didZero = true;
+    } else {
+      rangeAlert.set(true);
+      logger.error("Biscuit overextended! Shutting down movement!");
+      configurator.apply(BiscuitConstants.disableTalon());
+    }
+  }
+}

src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java

@@ -1,3 +1,62 @@
+// Blessed by the great tech-priests of the Adeptus Mechanicus
+
 package frc.robot.subsystems.biscuit;
 
-public class BiscuitSubsystem {}
+import static edu.wpi.first.units.Units.Rotations;
+
+import edu.wpi.first.units.measure.*;
+import frc.robot.constants.BiscuitConstants;
+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 BiscuitSubsystem extends MeasurableSubsystem {
+
+  private BiscuitIO io;
+  private BiscuitIOInputsAutoLogged inputs = new BiscuitIOInputsAutoLogged();
+  private Angle setPoint;
+
+  public void setPosition(Angle position) {
+    io.setPosition(position);
+    setPoint = position;
+  }
+
+  public Angle getPosition(Angle position) {
+    return inputs.position;
+  }
+
+  public AngularVelocity getVelocity(AngularVelocity velocity) {
+    return inputs.velocity;
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs(getName(), inputs);
+    Logger.recordOutput("Biscuit setPoint", setPoint);
+    Logger.recordOutput("Is Biscuit Finished", isFinished() ? 1.0 : 0.0);
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetry) {
+    super.registerWith(telemetry);
+    io.registerWith(telemetry);
+  }
+
+  public boolean isFinished() {
+    return setPoint.minus(inputs.position).abs(Rotations) <= BiscuitConstants.kCloseEnough;
+  }
+
+  @Override
+  public Set<Measure> getMeasures() {
+    return Set.of(
+        new Measure("Is Biscuit Finished", () -> isFinished() ? 1.0 : 0.0),
+        new Measure("Biscuit Set Point", () -> setPoint.in(Rotations)));
+  }
+
+  public void zero() {
+    io.zero();
+  }
+}