Add trap score button + routine

StuyPulse · Mar 12, 2024 · edf7ee1 · edf7ee1
1 parent 31297d2
commit edf7ee1
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 10 deletions.
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -168,14 +168,13 @@ private void configureDriverBindings() {
             .onTrue(new AmperScoreTrap())
             .onFalse(new AmperStop());
 
-        driver.getDPadUp()
-            .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(0)));
         driver.getDPadRight()
-            .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(90)));
-        driver.getDPadDown()
-            .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(180)));
-        driver.getDPadLeft()
-            .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(270)));
+            .whileTrue(new TrapScoreRoutine())
+            // .onFalse(new AmperToHeight(Lift.MIN_HEIGHT))
+            .onFalse(new AmperStop());
+
+        driver.getDPadUp()
+            .onTrue(new SwerveDriveResetHeading());
 
         driver.getTopButton()
             // on command start

diff --git a/src/main/java/com/stuypulse/robot/commands/TrapScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/TrapScoreRoutine.java
@@ -0,0 +1,26 @@
+/************************ PROJECT IZZI *************************/
+/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */
+/* Use of this source code is governed by an MIT-style license */
+/* that can be found in the repository LICENSE file.           */
+/***************************************************************/
+
+package com.stuypulse.robot.commands;
+
+import com.stuypulse.robot.commands.amper.AmperScoreTrap;
+import com.stuypulse.robot.commands.amper.AmperToHeight;
+import com.stuypulse.robot.commands.conveyor.ConveyorToAmp;
+import com.stuypulse.robot.constants.Settings.Amper.Lift;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class TrapScoreRoutine extends SequentialCommandGroup {
+
+    public TrapScoreRoutine() {
+        addCommands(
+            // NOTE: will not work if lift is at safe height, maybe this shouldn't run
+            new ConveyorToAmp(),
+            AmperToHeight.untilDone(Lift.TRAP_SCORE_HEIGHT),
+            new AmperScoreTrap()
+        );
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -88,7 +88,7 @@ public interface Amper {
 
         public interface Score {
             double AMP_SPEED = 1.0;
-            double TRAP_SPEED = 0.5;
+            double TRAP_SPEED = 0.3;
             double FROM_CONVEYOR_SPEED = 0.35;
             double TO_CONVEYOR_SPEED = 1.0;
 
@@ -103,7 +103,7 @@ public interface Lift {
             double CARRIAGE_MASS = 10; // kg
 
             double MIN_HEIGHT = 0;
-            double MAX_HEIGHT = Units.inchesToMeters(27.0); // amp 14.75
+            double MAX_HEIGHT = Units.inchesToMeters(27.0 - 2.0); // amp 14.75
 
             double VISUALIZATION_MIN_LENGTH = 0.5;
             Rotation2d ANGLE_TO_GROUND = Rotation2d.fromDegrees(68.02);
@@ -114,7 +114,7 @@ public interface Lift {
             double ACCEL_LIMIT = 2.0;
 
             double AMP_SCORE_HEIGHT = 0.34;
-            double TRAP_SCORE_HEIGHT = 0.60; //TODO: Tune this value
+            double TRAP_SCORE_HEIGHT = MAX_HEIGHT; //TODO: Tune this value
 
             public interface Encoder {
                 double GEARING = 1.0 / 9.0;