Merge pull request #38 from chopshop-166/Level-Four-Score

ScoreL4 on Coldstart
chopshop-166 · Feb 8, 2025 · 1ab60e2 · 1ab60e2
2 parents ec35e96 + 34939c9
commit 1ab60e2
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/CommandSequences.java b/src/main/java/frc/robot/CommandSequences.java
@@ -1,6 +1,7 @@
 package frc.robot;
 
 import static edu.wpi.first.wpilibj2.command.Commands.runOnce;
+import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds;
 
 import com.chopshop166.chopshoplib.controls.ButtonXboxController;
 
@@ -72,6 +73,17 @@ public Command scoreL1() {
                 led.elevatorAtPreset());
     }
 
+    // Scores on L4 preset, moving elevator up to score and then stowing elevator
+
+    public Command scoreL4() {
+        return coralManip.scoreL4().andThen(led.elevatorToPreset(),
+                // kick elevator up so that it actually scores (knocks coral onto reef)
+                elevator.moveTo(ElevatorPresets.HIGHESTPOINT),
+                waitSeconds(0.5),
+                elevator.moveTo(ElevatorPresets.STOW),
+                led.elevatorAtPreset());
+    }
+
     // Sets the rumble amount on controllers
 
     public Command setRumble(ButtonXboxController controller, int rumbleAmount) {

diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java
@@ -164,7 +164,7 @@ public ElevatorMap getElevatorMap() {
         ElevatorFeedforward feedForward = new ElevatorFeedforward(0, 0, 0);
 
         return new ElevatorMap(leftMotor, leftMotor.getEncoder(),
-                new ElevatorMap.ElevatorPresetValues(19.5, 5, 18, 38, 0),
+                new ElevatorMap.ElevatorPresetValues(19.5, 5, 18, 38, 0, 0, 0),
                 new ValueRange(0, 56), new ValueRange(3, 53), pid, feedForward);
     }
 

diff --git a/src/main/java/frc/robot/maps/subsystems/ElevatorMap.java b/src/main/java/frc/robot/maps/subsystems/ElevatorMap.java
@@ -25,14 +25,19 @@ public enum ElevatorPresets {
 
         SCOREL3,
 
+        SCOREL4,
+
+        HIGHESTPOINT,
+
         STOW,
 
         HOLD
     }
 
-    public record ElevatorPresetValues(double intake, double scoreL1, double scoreL2, double scoreL3, double stow) {
+    public record ElevatorPresetValues(double intake, double scoreL1, double scoreL2, double scoreL3, double scoreL4,
+            double highestPoint, double stow) {
         public ElevatorPresetValues() {
-            this(0, 0, 0, 0, 0);
+            this(0, 0, 0, 0, 0, 0, 0);
         }
 
         public double getValue(ElevatorPresets preset) {
@@ -47,6 +52,10 @@ public double getValue(ElevatorPresets preset) {
                     return scoreL2;
                 case SCOREL3:
                     return scoreL3;
+                case SCOREL4:
+                    return scoreL4;
+                case HIGHESTPOINT:
+                    return highestPoint;
                 case STOW:
                     return stow;
                 default:

diff --git a/src/main/java/frc/robot/subsystems/CoralManip.java b/src/main/java/frc/robot/subsystems/CoralManip.java
@@ -27,6 +27,13 @@ public Command scoreL1() {
         }).until(() -> !getData().gamePieceDetected).andThen(waitSeconds(RELEASE_DELAY), safeStateCmd());
     }
 
+    public Command scoreL4() {
+        return run(() -> {
+            getData().leftMotor.setpoint = RELEASE_SPEEDLEFT;
+            getData().rightMotor.setpoint = RELEASE_SPEEDLEFT;
+        }).until(() -> !getData().gamePieceDetected).andThen(waitSeconds(RELEASE_DELAY), safeStateCmd());
+    }
+
     public Command score() {
         return run(() -> {
             getData().leftMotor.setpoint = RELEASE_SPEEDRIGHT;