Merge branch 'staging' into autoRoutines

flamingchickens1540 · Feb 6, 2024 · 4ac895b · 4ac895b
2 parents eba9b83 + 46ae2ef
commit 4ac895b
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 7 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -65,9 +65,12 @@ public static class Drivetrain {
     }
 
     public static class Indexer {
-        // TODO: fix these constants
+
+        public static final int BEAM_BREAK_ID = 0;
         public static final int INTAKE_ID = 11;
         public static final int FEEDER_ID = 12;
+
+        // TODO: fix these constants
         public static final double FEEDER_KP = 0.5;
         public static final double FEEDER_KI = 0.1;
         public static final double FEEDER_KD = 0.001;
@@ -77,7 +80,7 @@ public static class Indexer {
         public static final double INTAKE_GEAR_RATIO = 1.0;
         public static final double INTAKE_MOI = 0.025;
         public static final double FEEDER_MOI = 0.025;
-        public static final int BEAM_BREAK_ID = 0;
+
 
 
     }
@@ -217,10 +220,11 @@ public enum ElevatorState {
     }
 
     public static class Tramp {
+        public static final int TRAMP_BEAM_BREAK_CHANNEL = 1;
         public static final double GEAR_RATIO = 3.0;
         public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D
-        public static final int TRAMP_MOTOR_ID = -1; //TODO: Configure this later
-        public static final int TRAMP_BEAM_BREAK_CHANNEL = -1; //TODO: Configure this later
+        public static final int MOTOR_ID = -1; //TODO: Configure this later
+
 
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/TrampShoot.java b/src/main/java/org/team1540/robot2024/commands/TrampShoot.java
@@ -0,0 +1,22 @@
+package org.team1540.robot2024.commands;
+
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+import org.team1540.robot2024.subsystems.tramp.Tramp;
+
+public class TrampShoot extends ParallelDeadlineGroup {
+    public TrampShoot(Tramp tramp) {
+        super(
+                Commands.sequence(
+                        Commands.waitUntil(() -> !tramp.isNoteStaged()),
+                        Commands.waitSeconds(2) //TODO: tune this
+                ),
+                Commands.startEnd(
+                        () -> tramp.setPercent(0.5), //TODO: tune this
+                        () -> tramp.stop(),
+                        tramp
+            )
+        );
+    }
+}
+
diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java
@@ -17,7 +17,7 @@ public StageTrampCommand(Tramp tramp, Indexer indexer) {
 
     @Override
     public void initialize() {
-        tramp.setPercent(0.5);
+        tramp.setPercent(0.5); //TODO: Tune this
         indexer.setFeederVelocity(-600);
         indexer.setIntakePercent(0.5);
     }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java
@@ -7,9 +7,9 @@
 import org.team1540.robot2024.Constants.Tramp;
 
 public class TrampIOSparkMax implements TrampIO {
-    private final DigitalInput beamBreak = new DigitalInput(0);
+    private final DigitalInput beamBreak = new DigitalInput(Tramp.TRAMP_BEAM_BREAK_CHANNEL);
     //TODO: Potentially change name :D
-    private final CANSparkMax motor = new CANSparkMax(Tramp.TRAMP_MOTOR_ID, MotorType.kBrushless);
+    private final CANSparkMax motor = new CANSparkMax(Tramp.MOTOR_ID, MotorType.kBrushless);
     private final RelativeEncoder motorEncoder = motor.getEncoder();
 
     public TrampIOSparkMax() {