Merge pull request #8 from strykeforce/algae

Initial Algae

Author: mwitcpalek

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

@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.algae.OpenLoopAlgaeCommand;
 import frc.robot.commands.coral.EnableEjectBeamCommand;
 import frc.robot.commands.coral.OpenLoopCoralCommand;
 import frc.robot.commands.drive.DriveTeleopCommand;
@@ -21,6 +22,8 @@
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.RobotConstants;
 import frc.robot.controllers.FlyskyJoystick;
+import frc.robot.subsystems.algae.AlgaeIOFX;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
 import frc.robot.subsystems.coral.CoralIO;
 import frc.robot.subsystems.coral.CoralIOFX;
 import frc.robot.subsystems.coral.CoralSubsystem;
@@ -33,22 +36,29 @@
 import org.strykeforce.telemetry.TelemetryService;
 
 public class RobotContainer {
-  private Swerve swerve;
-  private DriveSubsystem driveSubsystem;
 
   private ElevatorIO elevatorIO;
   private ElevatorSubsystem elevatorSubsystem;
 
   private final CoralIO coralIO;
   private final CoralSubsystem coralSubsystem;
 
+  private AlgaeIOFX algaeIO;
+  private AlgaeSubsystem algaeSubsystem;
+
+  private Swerve swerve;
+  private DriveSubsystem driveSubsystem;
+
   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);
 
   public RobotContainer() {
+    algaeIO = new AlgaeIOFX();
+    algaeSubsystem = new AlgaeSubsystem(algaeIO);
+
     swerve = new Swerve();
     driveSubsystem = new DriveSubsystem(swerve);
 
@@ -66,6 +76,8 @@ public RobotContainer() {
   private void configureTelemetry() {
     telemetryService.register(driveSubsystem);
     telemetryService.register(coralSubsystem);
+    telemetryService.register(algaeSubsystem);
+    telemetryService.register(elevatorSubsystem);
     telemetryService.start();
   }
 
@@ -103,6 +115,12 @@ private void configureOperatorBindings() {
     // Zero Elevator
     new JoystickButton(xboxController, XboxController.Button.kX.value)
         .onTrue(new ZeroElevatorCommand(elevatorSubsystem));
+
+    // Algae Buttons
+    new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value)
+        .onTrue(new OpenLoopAlgaeCommand(algaeSubsystem, 0.5));
+    new JoystickButton(xboxController, XboxController.Button.kRightBumper.value)
+        .onTrue(new OpenLoopAlgaeCommand(algaeSubsystem, -0.5));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/algae/OpenLoopAlgaeCommand.java

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

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

@@ -1,7 +1,6 @@
 package frc.robot.constants;
 
-import static edu.wpi.first.units.Units.Degrees;
-import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
@@ -16,16 +15,19 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
 import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
-import edu.wpi.first.units.measure.*;
+import edu.wpi.first.units.measure.AngularVelocity;
 
 public class AlgaeConstants {
+  public static int kFxId = 30;
 
-  public static int kFxId = 4; // CHANGE TO MOTOR NUMBER
+  public static final AngularVelocity kCloseEnough = RotationsPerSecond.of(0.1);
+  public static final AngularVelocity kMaxFwd = RotationsPerSecond.of(100);
+  public static final AngularVelocity kMaxRev = RotationsPerSecond.of(-100);
 
-  public static final Angle kCloseEnough = Degrees.of(5);
-  public static final Angle kMaxFwd = Rotations.of(100);
-  public static final Angle kMaxRev = Rotations.of(-100);
-  public static final Angle kZeroTicks = Rotations.of(1530);
+  public static final AngularVelocity kHoldSpeed = RotationsPerSecond.of(-0.1);
+  public static final AngularVelocity kBargeScoreSpeed = RotationsPerSecond.of(1);
+  public static final AngularVelocity kProcessorScoreSpeed = RotationsPerSecond.of(1);
+  public static final AngularVelocity kIntakingSpeed = RotationsPerSecond.of(-1);
 
   // Example Talon FX Config
   public static TalonFXConfiguration getFXConfig() {
@@ -57,9 +59,9 @@ public static TalonFXConfiguration getFXConfig() {
     SoftwareLimitSwitchConfigs swLimit =
         new SoftwareLimitSwitchConfigs()
             .withForwardSoftLimitEnable(true)
-            .withForwardSoftLimitThreshold(kMaxFwd)
+            .withForwardSoftLimitThreshold(kMaxFwd.in(RotationsPerSecond))
             .withReverseSoftLimitEnable(true)
-            .withReverseSoftLimitThreshold(kMaxRev);
+            .withReverseSoftLimitThreshold(kMaxRev.in(RotationsPerSecond));
     fxConfig.SoftwareLimitSwitch = swLimit;
 
     Slot0Configs slot0 =

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

@@ -93,7 +93,7 @@ public final class VisionConstants {
       VecBuilder.fill(Units.degreesToRadians(0.01));
 
   // Increase these numbers to trust global measurements from vision less. This matrix is in the
-  // form [x, y, theta]ᵀ, with units in meters and radians.
+  // form [x, y, theta], with units in meters and radians.
   // Vision Odometry Standard devs
   public static Matrix<N3, N1> kVisionMeasurementStdDevs =
       VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360));

src/main/java/frc/robot/subsystems/algae/algaeIO.java renamed to src/main/java/frc/robot/subsystems/algae/AlgaeIO.java

@@ -1,22 +1,22 @@
 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;
 
-public interface algaeIO {
+public interface AlgaeIO {
   @AutoLog
-  public static class IOInputs {
+  public static class AlgaeIOInputs {
     public AngularVelocity velocity;
-    public Angle location;
-    public ReverseLimitValue reverseLimitSwitch;
+    public boolean isFwdLimitSwitchClosed;
+    public boolean isRevLimitSwitchClosed;
   }
 
-  public default void updateInputs(IOInputs inputs) {}
+  public default void updateInputs(AlgaeIOInputs inputs) {}
 
-  public default void setPosition(double position) {}
+  public default void setSpeed(AngularVelocity speed) {}
+
+  public default void setPct(double pct) {}
 
   public default void zero() {}
 

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

@@ -0,0 +1,62 @@
+package frc.robot.subsystems.algae;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+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.AlgaeConstants;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
+
+public class AlgaeIOFX implements AlgaeIO {
+  private Logger logger;
+  private TalonFX talonFX;
+  private AlgaeIOInputs inputs;
+
+  // FX Access objects
+  private StatusSignal<AngularVelocity> curVelocity;
+  private StatusSignal<ForwardLimitValue> fwdLimitSwitch;
+  private StatusSignal<ReverseLimitValue> revLimitSwitch;
+  private VelocityVoltage speedRequest = new VelocityVoltage(0).withEnableFOC(false).withSlot(0);
+  private DutyCycleOut dutyCycleRequest = new DutyCycleOut(0).withEnableFOC(false);
+
+  public AlgaeIOFX() {
+    logger = LoggerFactory.getLogger(this.getClass());
+    talonFX = new TalonFX(AlgaeConstants.kFxId);
+    fwdLimitSwitch = talonFX.getForwardLimit();
+    revLimitSwitch = talonFX.getReverseLimit();
+    curVelocity = talonFX.getVelocity();
+  }
+
+  @Override
+  public void updateInputs(AlgaeIOInputs inputs) {
+    BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch);
+    inputs.velocity = curVelocity.refresh().getValue();
+    inputs.isFwdLimitSwitchClosed = fwdLimitSwitch.getValue().value == 1; // FIXME check right value
+    inputs.isRevLimitSwitchClosed = revLimitSwitch.getValue().value == 0; // FIXME check right value
+  }
+
+  @Override
+  public void setSpeed(AngularVelocity speed) {
+    talonFX.setControl(speedRequest.withVelocity(speed));
+  }
+
+  @Override
+  public void setPct(double pct) {
+    talonFX.setControl(dutyCycleRequest.withOutput(pct));
+  }
+
+  public AngularVelocity AngularVelocity() {
+    return talonFX.getVelocity().getValue();
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    telemetryService.register(talonFX, true);
+  }
+}

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

@@ -1,26 +1,119 @@
 package frc.robot.subsystems.algae;
 
-public class AlgaeSubsystem {
+import static edu.wpi.first.units.Units.RotationsPerSecond;
 
-  public static final String kStowSpeed = null;
+import edu.wpi.first.units.measure.AngularVelocity;
+import frc.robot.constants.AlgaeConstants;
+import frc.robot.standards.ClosedLoopSpeedSubsystem;
+import frc.robot.subsystems.algae.AlgaeIO.AlgaeIOInputs;
+import java.util.Set;
+import org.littletonrobotics.junction.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
 
-  public boolean hasAlgae() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'hasAlgae'");
+public class AlgaeSubsystem extends MeasurableSubsystem implements ClosedLoopSpeedSubsystem {
+  private org.slf4j.Logger logger = LoggerFactory.getLogger(AlgaeSubsystem.class);
+
+  private final AlgaeIO io;
+  private final AlgaeIOInputs inputs = new AlgaeIOInputs();
+  private AngularVelocity desiredSpeed;
+
+  private AlgaeStates curState = AlgaeStates.IDLE;
+
+  public AlgaeSubsystem(AlgaeIO io) {
+    this.io = io;
+  }
+
+  public AlgaeStates getState() {
+    return curState;
+  }
+
+  public void setState(AlgaeStates newState) {
+    logger.info("{} -> {}", curState, newState);
+    curState = newState;
   }
 
   public void intake() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'intake'");
+    setSpeed(AlgaeConstants.kIntakingSpeed);
   }
 
   public void scoreProcessor() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'scoreProcessor'");
+    setSpeed(AlgaeConstants.kProcessorScoreSpeed);
   }
 
   public void scoreBarge() {
+    setSpeed(AlgaeConstants.kBargeScoreSpeed);
+  }
+
+  public void hold() {
+    setSpeed(AlgaeConstants.kHoldSpeed);
+  }
+
+  @Override
+  public void setSpeed(AngularVelocity speed) {
+    io.setSpeed(speed);
+    desiredSpeed = speed;
+  }
+
+  public void setPct(double pct) {
+    io.setPct(pct);
+  }
+
+  @Override
+  public AngularVelocity getSpeed() {
+    return inputs.velocity;
+  }
+
+  @Override
+  public boolean atSpeed() {
+    return inputs.velocity.minus(desiredSpeed).abs(RotationsPerSecond)
+        < AlgaeConstants.kCloseEnough.in(RotationsPerSecond);
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.recordOutput("Algae/state", curState);
+    Logger.recordOutput("Algae/setpoint", desiredSpeed.in(RotationsPerSecond));
+
+    switch (curState) {
+      case HAS_ALGAE -> {
+        if (!inputs.isFwdLimitSwitchClosed) {
+          setState(AlgaeStates.EMPTY);
+          setSpeed(RotationsPerSecond.of(0));
+        }
+      }
+      case EMPTY -> {
+        if (inputs.isRevLimitSwitchClosed) { // FIXME: correct?
+          hold();
+          setState(AlgaeStates.HAS_ALGAE);
+        }
+      }
+      case IDLE -> {}
+    }
+  }
+
+  @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 AlgaeStates {
+    HAS_ALGAE,
+    EMPTY,
+    IDLE
+  }
+
+  public boolean hasAlgae() {
     // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'scoreBarge'");
+    throw new UnsupportedOperationException("Unimplemented method 'hasAlgae'");
   }
 }