Merge pull request #24 from Team334/sim-dio

Add digital input sim
Team334 · Jan 20, 2025 · fea7ee7 · fea7ee7
2 parents 9227b98 + 424ad2a
commit fea7ee7
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Serializer.java b/src/main/java/frc/robot/subsystems/Serializer.java
@@ -2,21 +2,38 @@
 
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.simulation.DIOSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.AdvancedSubsystem;
 import frc.robot.Constants.SerializerConstants;
+import frc.robot.Robot;
 
 public class Serializer extends AdvancedSubsystem {
-  private DigitalInput _frontBeam;
-  private DigitalInput _backBeam;
+  private final DigitalInput _frontBeam;
+  private final DigitalInput _backBeam;
+
+  private final DIOSim _frontBeamSim;
+  private final DIOSim _backBeamSim;
 
   public Serializer() {
     setDefaultCommand(setSpeed(0));
 
     _frontBeam = new DigitalInput(SerializerConstants.frontBeamPort);
     _backBeam = new DigitalInput(SerializerConstants.backBeamPort);
 
+    if (Robot.isSimulation()) {
+      _frontBeamSim = new DIOSim(_frontBeam);
+      _backBeamSim = new DIOSim(_backBeam);
+
+      SmartDashboard.putBoolean("Front Beam Value", getBackBeam());
+      SmartDashboard.putBoolean("Back Beam Value", getBackBeam());
+    } else {
+      _frontBeamSim = null;
+      _backBeamSim = null;
+    }
+
     new Trigger(this::getFrontBeam).whileTrue(holdCoral());
   }
 
@@ -48,6 +65,15 @@ public void periodic() {
     super.periodic();
   }
 
+  @Override
+  public void simulationPeriodic() {
+    super.simulationPeriodic();
+
+    // TODO: switch to tuning class once that's made
+    _frontBeamSim.setValue(SmartDashboard.getBoolean("Front Beam Value", getFrontBeam()));
+    _backBeamSim.setValue(SmartDashboard.getBoolean("Back Beam Value", getBackBeam()));
+  }
+
   @Override
   public void close() {
     _frontBeam.close();

diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java
@@ -7,13 +7,16 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.simulation.DIOSim;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.AdvancedSubsystem;
 import frc.robot.Constants;
 import frc.robot.Constants.WristevatorConstants;
@@ -74,10 +77,23 @@ public Distance getHeight() {
 
   private final Consumer<WristevatorSetpoint> _wristevatorSetpointSetter;
 
-  private DigitalInput _elevatorSwitch = new DigitalInput(WristevatorConstants.elevatorSwitchPort);
+  private final DigitalInput _elevatorSwitch =
+      new DigitalInput(WristevatorConstants.elevatorSwitchPort);
+
+  private final DIOSim _elevatorSwitchSim;
 
   public Wristevator(Consumer<WristevatorSetpoint> wristevatorSetpointSetter) {
     _wristevatorSetpointSetter = wristevatorSetpointSetter;
+
+    if (Robot.isSimulation()) {
+      _elevatorSwitchSim = new DIOSim(_elevatorSwitch);
+
+      new Trigger(() -> getHeight() == 0)
+          .onTrue(Commands.runOnce(() -> _elevatorSwitchSim.setValue(true)))
+          .onFalse(Commands.runOnce(() -> _elevatorSwitchSim.setValue(false)));
+    } else {
+      _elevatorSwitchSim = null;
+    }
   }
 
   @Logged(name = "Height")