Feat: added beam breaks

flamingchickens1540 · Jun 12, 2024 · a09f5b9 · a09f5b9
1 parent 21d64a2
commit a09f5b9
Show file tree

Hide file tree

Showing 5 changed files with 30 additions and 9 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -89,7 +89,9 @@ public static class Auto {
     }
 
     public static class Indexer {
-        public static final int BEAM_BREAK_ID = IS_COMPETITION_ROBOT ? 7 : 8;
+        public static final int BEAM_BREAK_BACK_ID = IS_COMPETITION_ROBOT ? 7 : 8;
+        public static final int BEAM_BREAK_FRONT_ID = 0; // TODO: update id
+        public static final int BEAM_BREAK_SHOOTER_ID = 0; // TODO: update id
         public static final int INTAKE_ID = 13;
         public static final int FEEDER_ID = 15;
 

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -67,8 +67,16 @@ public void setIntakePercent(double percent) {
         io.setIntakeVoltage(percent * 12.0);
     }
 
-    public boolean isNoteStaged() {
-        return inputs.noteInIntake;
+    public boolean isNoteStagedBack() {
+        return inputs.noteInIntakeBack;
+    }
+
+    public boolean isNoteStagedFront() {
+        return inputs.noteInIntakeFront;
+    }
+
+    public boolean isNoteStagedShooter() {
+        return inputs.noteInIntakeShooter;
     }
 
     public void setFeederVelocity(double setpointRPM) {
@@ -120,7 +128,7 @@ public Command moveNoteOut() {
                 () -> setIntakePercent(-1),
                 () -> {},
                 (interrupted) -> stopIntake(),
-                () -> !isNoteStaged(),
+                () -> !isNoteStagedBack(),
                 this
         );
     }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java
@@ -14,7 +14,9 @@ class IndexerIOInputs {
         public double feederCurrentAmps = 0.0;
         public double feederVelocityRPM = 0.0;
         public double feederTempCelsius = 0.0;
-        public boolean noteInIntake = false;
+        public boolean noteInIntakeFront = false;
+        public boolean noteInIntakeBack = false;
+        public boolean noteInIntakeShooter = false;
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java
@@ -15,7 +15,9 @@ 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 DigitalInput indexerBeamBreakFront = new DigitalInput(BEAM_BREAK_FRONT_ID);
+    private final DigitalInput indexerBeamBreakBack = new DigitalInput(BEAM_BREAK_BACK_ID);
+    private final DigitalInput indexerBeamBreakShooter = new DigitalInput(BEAM_BREAK_SHOOTER_ID);
     private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI, FEEDER_KD);
     private boolean isClosedLoop = true;
     private double intakeVoltage = 0.0;
@@ -39,7 +41,9 @@ public void updateInputs(IndexerIOInputs inputs) {
         inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps();
         inputs.feederVoltage = feederVoltage;
         inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM();
-        inputs.noteInIntake = indexerBeamBreak.get();
+        inputs.noteInIntakeFront = indexerBeamBreakFront.get();
+        inputs.noteInIntakeBack = indexerBeamBreakBack.get();
+        inputs.noteInIntakeShooter = indexerBeamBreakShooter.get();
     }
 
     @Override

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java
@@ -9,7 +9,9 @@
 public class IndexerIOSparkMax implements IndexerIO {
     private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless);
     private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless);
-    private final DigitalInput indexerBeamBreak = new DigitalInput(7);
+    private final DigitalInput indexerBeamBreakFront = new DigitalInput(0); // TODO: update id
+    private final DigitalInput indexerBeamBreakBack = new DigitalInput(7);
+    private final DigitalInput indexerBeamBreakShooter = new DigitalInput(0); // TODO: update id
 
     private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder();
     private final RelativeEncoder feederEncoder = feederMotor.getEncoder();
@@ -43,7 +45,10 @@ public void updateInputs(IndexerIOInputs inputs) {
         inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput();
         inputs.feederVelocityRPM = feederEncoder.getVelocity();
         inputs.feederTempCelsius = feederMotor.getMotorTemperature();
-        inputs.noteInIntake = !indexerBeamBreak.get();
+        inputs.noteInIntakeFront = !indexerBeamBreakFront.get();
+        inputs.noteInIntakeBack = !indexerBeamBreakBack.get();
+        inputs.noteInIntakeShooter = !indexerBeamBreakShooter.get();
+
     }
 
     @Override