Initial Coral Subsystem

src/main/java/frc/robot/Robot.java

@@ -6,7 +6,11 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.constants.BuildConstants;
 import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
 public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
@@ -17,6 +21,31 @@ public Robot() {
     m_robotContainer = new RobotContainer();
   }
 
+  @Override
+  public void robotInit() {
+    if (isReal()) {
+      Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+      Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+      Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+      Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+      Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+      switch (BuildConstants.DIRTY) {
+        case 0:
+          Logger.recordMetadata("GitDirty", "All Changes Committed");
+          break;
+        case 1:
+          Logger.recordMetadata("GitDirty", "Uncommitted changes");
+          break;
+        default:
+          Logger.recordMetadata("GitDirty", "Unknown");
+          break;
+      }
+      Logger.addDataReceiver(new WPILOGWriter("/home/lvuser/logs"));
+      Logger.addDataReceiver(new NT4Publisher());
+    }
+    Logger.start();
+  }
+
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();

src/main/java/frc/robot/RobotContainer.java

@@ -8,8 +8,14 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.commands.coral.EnableEjectBeamCommand;
+import frc.robot.commands.coral.OpenLoopCoralCommand;
 import frc.robot.commands.drive.DriveTeleopCommand;
 import frc.robot.controllers.FlyskyJoystick;
+import frc.robot.subsystems.coral.CoralIO;
+import frc.robot.subsystems.coral.CoralIOFX;
+import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.drive.Swerve;
 import org.strykeforce.telemetry.TelemetryController;
@@ -21,25 +27,53 @@ public class RobotContainer {
 
   private final XboxController xboxController = new XboxController(1);
   private final Joystick driveJoystick = new Joystick(0);
+
   private final FlyskyJoystick flysky = new FlyskyJoystick(driveJoystick);
   private final TelemetryService telemetryService = new TelemetryService(TelemetryController::new);
 
+  private final CoralSubsystem coralSubsystem;
+  private final CoralIO coralIO;
+
   public RobotContainer() {
     swerve = new Swerve();
     driveSubsystem = new DriveSubsystem(swerve);
 
-    configureBindings();
+    coralIO = new CoralIOFX();
+    coralSubsystem = new CoralSubsystem(coralIO);
+
+    configureTelemetry();
     configureDriverBindings();
+    configureOperatorBindings();
   }
 
-  private void configureBindings() {}
+  private void configureTelemetry() {
+    telemetryService.register(driveSubsystem);
+    telemetryService.register(coralSubsystem);
+    telemetryService.start();
+  }
 
   private void configureDriverBindings() {
     driveSubsystem.setDefaultCommand(
         new DriveTeleopCommand(
             () -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem));
   }
 
+  private void configureOperatorBindings() {
+    // Stop Coral
+    new JoystickButton(xboxController, XboxController.Button.kB.value)
+        .onTrue(new OpenLoopCoralCommand(coralSubsystem, 0));
+
+    // Intake Coral
+    new JoystickButton(xboxController, XboxController.Button.kY.value)
+        .onTrue(new OpenLoopCoralCommand(coralSubsystem, -0.5))
+        .onTrue(new EnableEjectBeamCommand(false, coralSubsystem));
+
+    // Eject Coral
+    new JoystickButton(xboxController, XboxController.Button.kA.value)
+        .onTrue(new OpenLoopCoralCommand(coralSubsystem, -0.5))
+        .onTrue(new EnableEjectBeamCommand(true, coralSubsystem));
+  }
+
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
   }

src/main/java/frc/robot/commands/coral/EnableEjectBeamCommand.java

@@ -0,0 +1,19 @@
+package frc.robot.commands.coral;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.coral.CoralSubsystem;
+
+public class EnableEjectBeamCommand extends InstantCommand {
+  private CoralSubsystem coralSubsystem;
+  private Boolean enable;
+
+  public EnableEjectBeamCommand(Boolean enable, CoralSubsystem coralSubsystem) {
+    this.coralSubsystem = coralSubsystem;
+    this.enable = enable;
+  }
+
+  @Override
+  public void initialize() {
+    coralSubsystem.enableEjectBeam(enable);
+  }
+}

src/main/java/frc/robot/commands/coral/OpenLoopCoralCommand.java

@@ -0,0 +1,19 @@
+package frc.robot.commands.coral;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.coral.CoralSubsystem;
+
+public class OpenLoopCoralCommand extends InstantCommand {
+  private CoralSubsystem coralSubsystem;
+  private double pct;
+
+  public OpenLoopCoralCommand(CoralSubsystem coralSubsystem, double pct) {
+    this.coralSubsystem = coralSubsystem;
+    this.pct = pct;
+  }
+
+  @Override
+  public void initialize() {
+    coralSubsystem.setPct(pct);
+  }
+}

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

@@ -1,3 +1,82 @@
 package frc.robot.constants;
 
-public class CoralConstants {}
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+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.TalonFXSConfiguration;
+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 = RotationsPerSecond.of(0.1);
+
+  public static final AngularVelocity kIntakingSpeed = RotationsPerSecond.of(-1);
+  public static final AngularVelocity kEjectingSpeed = RotationsPerSecond.of(1);
+
+  // Coral Talon FX Config
+  public static TalonFXSConfiguration getFXConfig() {
+    TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration();
+
+    CurrentLimitsConfigs current =
+        new CurrentLimitsConfigs()
+            .withStatorCurrentLimit(10)
+            .withStatorCurrentLimitEnable(false)
+            .withSupplyCurrentLimit(10)
+            .withSupplyCurrentLowerLimit(8)
+            .withSupplyCurrentLowerTime(0.02)
+            .withSupplyCurrentLimitEnable(true);
+    fxsConfig.CurrentLimits = current;
+
+    HardwareLimitSwitchConfigs hwLimit =
+        new HardwareLimitSwitchConfigs()
+            .withForwardLimitAutosetPositionEnable(false)
+            .withForwardLimitEnable(false)
+            .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
+            .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
+            .withReverseLimitAutosetPositionEnable(false)
+            .withReverseLimitEnable(true)
+            .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
+            .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
+    fxsConfig.HardwareLimitSwitch = hwLimit;
+
+    Slot0Configs slot0 =
+        new Slot0Configs()
+            .withKP(0)
+            .withKI(0)
+            .withKD(0)
+            .withGravityType(GravityTypeValue.Elevator_Static)
+            .withKG(0)
+            .withKS(0)
+            .withKV(0)
+            .withKA(0);
+    fxsConfig.Slot0 = slot0;
+
+    MotionMagicConfigs motionMagic =
+        new MotionMagicConfigs()
+            .withMotionMagicAcceleration(0)
+            .withMotionMagicCruiseVelocity(0)
+            .withMotionMagicExpo_kA(0)
+            .withMotionMagicExpo_kV(0)
+            .withMotionMagicJerk(0);
+    fxsConfig.MotionMagic = motionMagic;
+
+    MotorOutputConfigs motorOut =
+        new MotorOutputConfigs()
+            .withDutyCycleNeutralDeadband(0.01)
+            .withNeutralMode(NeutralModeValue.Coast);
+    fxsConfig.MotorOutput = motorOut;
+
+    return fxsConfig;
+  }
+}

src/main/java/frc/robot/subsystems/algae/algaeIO.java

@@ -1,5 +1,7 @@
 package frc.robot.subsystems.algae;
 
+import com.ctre.phoenix6.signals.ReverseLimitValue;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import org.littletonrobotics.junction.AutoLog;
 import org.strykeforce.telemetry.TelemetryService;
@@ -8,6 +10,8 @@ public interface algaeIO {
   @AutoLog
   public static class IOInputs {
     public AngularVelocity velocity;
+    public Angle location;
+    public ReverseLimitValue reverseLimitSwitch;
   }
 
   public default void updateInputs(IOInputs inputs) {}

src/main/java/frc/robot/subsystems/algae/algaeIOFX.java

@@ -1,3 +1,62 @@
 package frc.robot.subsystems.algae;
 
-public class algaeIOFX {}
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.ReverseLimitValue;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import frc.robot.constants.AlgaeConstants;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
+
+/** Implementation of the algaeIO interface using TalonFX motors. */
+public class algaeIOFX implements algaeIO {
+  private Logger logger;
+  private final TalonFX talonFX;
+  private StatusSignal<AngularVelocity> currVelocity;
+  private StatusSignal<ReverseLimitValue> reverseLimitSwitch;
+  private StatusSignal<Angle> currAngle;
+
+  // FX Access objects
+  TalonFXConfigurator configurator;
+  private MotionMagicDutyCycle positionRequest =
+      new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
+
+  // Constructor to initialize the TalonFX motor controller
+  public algaeIOFX() {
+    logger = LoggerFactory.getLogger(this.getClass());
+    talonFX = new TalonFX(AlgaeConstants.kFxId);
+    reverseLimitSwitch = talonFX.getReverseLimit();
+    currVelocity = talonFX.getVelocity();
+    currAngle = talonFX.getPosition();
+  }
+
+  @Override
+  public void updateInputs(IOInputs inputs) {
+    // inputs.reverseLimitSwitch = reverseLimitSwitch.refresh().getValue().value == 1;
+    inputs.velocity = currVelocity.refresh().getValue();
+    inputs.location = currAngle.refresh().getValue();
+  }
+
+  @Override
+  public void setPosition(double position) {
+    talonFX.setPosition(position);
+  }
+
+  public Angle getPosition() {
+    return currAngle.getValue();
+  }
+
+  @Override
+  public void zero() {
+    // not doing one for now
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    telemetryService.register(talonFX, true);
+  }
+}

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

@@ -1,3 +1,29 @@
 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 isFwdBeamBroken = false;
+    public boolean isRevBeamBroken = false;
+  }
+
+  public default void setVelocity(AngularVelocity velocity) {}
+
+  public default void setPct(double percentOutput) {}
+
+  public default void enableFwdLimitSwitch(boolean enabled) {}
+
+  public default void enableRevLimitSwitch(boolean enabled) {}
+
+  public default void updateInputs(CoralIOInputs inputs) {}
+
+  public default void registerWith(TelemetryService telemetryService) {}
+}