feat: add beam breaks to sim

flamingchickens1540 · Mar 21, 2024 · 3fd8a41 · 3fd8a41
1 parent 0adf32a
commit 3fd8a41
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import org.team1540.robot2024.Constants;
 
@@ -14,6 +15,7 @@ public class IndexerIOSim implements IndexerIO {
     private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO, INTAKE_MOI);
     private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO, FEEDER_MOI);
     //    private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break");
+    private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID);
     private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI, FEEDER_KD);
     private boolean isClosedLoop = true;
     private double intakeVoltage = 0.0;
@@ -37,6 +39,7 @@ public void updateInputs(IndexerIOInputs inputs) {
         inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps();
         inputs.feederVoltage = feederVoltage;
         inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM();
+        inputs.noteInIntake = indexerBeamBreak.get();
     }
 
     @Override

diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java
@@ -2,22 +2,26 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import org.team1540.robot2024.Constants;
 
+import static org.team1540.robot2024.Constants.Tramp.BEAM_BREAK_CHANNEL;
 import static org.team1540.robot2024.Constants.Tramp.GEAR_RATIO;
 
 public class TrampIOSim implements TrampIO {
     private final DCMotorSim sim = new DCMotorSim(DCMotor.getNEO(1), GEAR_RATIO, 0.025); //TODO: Fix MOI its probably wrong :D
-
+    private final DigitalInput beamBreak = new DigitalInput(BEAM_BREAK_CHANNEL);
     private double appliedVolts = 0.0;
 
+
     @Override
     public void updateInputs(TrampIOInputs inputs) {
         sim.update(Constants.LOOP_PERIOD_SECS);
         inputs.velocityRPM = sim.getAngularVelocityRPM();
         inputs.appliedVolts = appliedVolts;
         inputs.currentAmps = sim.getCurrentDrawAmps();
+        inputs.noteInTramp =beamBreak.get();
     }
 
     @Override