added remaining DIO sim

Team334 · Jan 21, 2025 · 208efde · 208efde
1 parent c298572
commit 208efde
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.lib.AdvancedSubsystem;
-import frc.robot.Robot;
 
 public class Intake extends AdvancedSubsystem {
   private final Mechanism2d _mech = new Mechanism2d(1.85, 1);
@@ -45,12 +44,15 @@ public Command set(double actuatorAngle, double feedSpeed) {
   @Override
   public void periodic() {
     super.periodic();
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    super.simulationPeriodic();
 
-    if (Robot.isSimulation()) {
-      _intake.setAngle(Math.toDegrees(getAngle()));
+    _intake.setAngle(Math.toDegrees(getAngle()));
 
-      SmartDashboard.putData("Intake Visualizer", _mech);
-    }
+    SmartDashboard.putData("Intake Visualizer", _mech);
   }
 
   @Override

diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java
@@ -4,11 +4,14 @@
 
 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.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.AdvancedSubsystem;
 import frc.robot.Constants.ManipulatorConstants;
+import frc.robot.Robot;
 import java.util.function.Consumer;
 
 public class Manipulator extends AdvancedSubsystem {
@@ -17,6 +20,9 @@ public class Manipulator extends AdvancedSubsystem {
   private final DigitalInput _beam = new DigitalInput(ManipulatorConstants.beamPort);
   private final DigitalInput _limitSwitch = new DigitalInput(ManipulatorConstants.switchPort);
 
+  private final DIOSim _beamSim;
+  private final DIOSim _limitSwitchSim;
+
   private final Trigger _beamBroken = new Trigger(this::getBeam);
   private final Trigger _switchPressed = new Trigger(this::getSwitch);
 
@@ -28,6 +34,17 @@ public Manipulator(Consumer<Piece> currentPieceSetter) {
 
     setDefaultCommand(setSpeed(0));
 
+    if (Robot.isSimulation()) {
+      _beamSim = new DIOSim(_beam);
+      _limitSwitchSim = new DIOSim(_limitSwitch);
+
+      SmartDashboard.putBoolean("Beam Value", getBeam());
+      SmartDashboard.putBoolean("Limit Switch Value", getSwitch());
+    } else {
+      _beamSim = null;
+      _limitSwitchSim = null;
+    }
+
     new Trigger(() -> getCurrentPiece() == Piece.CORAL).whileTrue(holdCoral());
     new Trigger(() -> getCurrentPiece() == Piece.ALGAE).whileTrue(holdAlgae());
 
@@ -94,6 +111,14 @@ public void periodic() {
     super.periodic();
   }
 
+  @Override
+  public void simulationPeriodic() {
+    super.simulationPeriodic();
+
+    _beamSim.setValue(SmartDashboard.getBoolean("Beam Value", getBeam()));
+    _limitSwitchSim.setValue(SmartDashboard.getBoolean("Limit Switch Value", getSwitch()));
+  }
+
   @Override
   public void close() {}
 }
diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java
@@ -118,13 +118,14 @@ public Command setSpeeds(DoubleSupplier elevatorSpeed, DoubleSupplier wristSpeed
   @Override
   public void periodic() {
     super.periodic();
+  }
 
-    if (Robot.isSimulation()) {
-      _elevator.setLength(getHeight());
-      _wrist.setAngle(Math.toDegrees(getAngle()) - 90);
+  @Override
+  public void simulationPeriodic() {
+    _elevator.setLength(getHeight());
+    _wrist.setAngle(Math.toDegrees(getAngle()) - 90);
 
-      SmartDashboard.putData("Wristevator Visualizer", _mech);
-    }
+    SmartDashboard.putData("Wristevator Visualizer", _mech);
   }
 
   @Override