Merge branch 'main' into intake

# Conflicts: # src/main/java/org/team1540/robot2024/Constants.java
flamingchickens1540 · Feb 7, 2024 · e3618c1 · e3618c1
2 parents e7e23b5 + 09b7733
commit e3618c1
Show file tree

Hide file tree

Showing 6 changed files with 41 additions and 13 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -4,6 +4,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -16,7 +17,7 @@
 public final class Constants {
     public static final boolean IS_COMPETITION_ROBOT = true;
     // Whether to pull PID constants from SmartDashboard
-    public static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP
+    private static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP
     private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY
 
     public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;
@@ -65,9 +66,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 +81,6 @@ 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;
         public static final int VELOCITY_ERR_TOLERANCE_RPM = 10;
 
 
@@ -218,10 +221,13 @@ 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
+    }
 
+    public static boolean isTuningMode() {
+        return tuningMode && !DriverStation.isFMSAttached();
     }
 }
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
@@ -16,7 +16,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/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -4,10 +4,10 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
+import org.team1540.robot2024.Constants;
 import org.team1540.robot2024.util.LoggedTunableNumber;
 
 import static org.team1540.robot2024.Constants.Indexer.*;
-import static org.team1540.robot2024.Constants.tuningMode;
 
 
 public class Indexer extends SubsystemBase {
@@ -25,7 +25,7 @@ public Indexer(IndexerIO io) {
     public void periodic() {
         io.updateInputs(inputs);
         Logger.processInputs("Indexer", inputs);
-        if (tuningMode) {
+        if (Constants.isTuningMode()) {
             if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
                 io.configureFeederPID(kP.get(), kI.get(), kD.get());
             }

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() {

diff --git a/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java b/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java
@@ -51,7 +51,7 @@ public void initDefault(double defaultValue) {
         if (!hasDefault) {
             hasDefault = true;
             this.defaultValue = defaultValue;
-            if (Constants.tuningMode) {
+            if (Constants.isTuningMode()) {
                 dashboardNumber = new LoggedDashboardNumber(key, defaultValue);
             }
         }
@@ -66,7 +66,7 @@ public double get() {
         if (!hasDefault) {
             return 0.0;
         } else {
-            return Constants.tuningMode ? dashboardNumber.get() : defaultValue;
+            return Constants.isTuningMode() ? dashboardNumber.get() : defaultValue;
         }
     }
 
@@ -79,7 +79,7 @@ public double get() {
      * otherwise.
      */
     public boolean hasChanged(int id) {
-        if (!Constants.tuningMode) return false;
+        if (!Constants.isTuningMode()) return false;
         double currentValue = get();
         Double lastValue = lastHasChangedValues.get(id);
         if (lastValue == null || currentValue != lastValue) {