wait to shoot until the elevator is up

only in teleop
Team2338 · Mar 3, 2025 · 9c96b7e · 9c96b7e
1 parent 6f496ae
commit 9c96b7e
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 4 deletions.
diff --git a/src/main/java/team/gif/robot/Constants.java b/src/main/java/team/gif/robot/Constants.java
@@ -278,7 +278,7 @@ public static final class Elevator{
         public static final double MAX_ACCELERATION = 130;
         public static final double REV_MAX_ACCELERATION = 80;
 
-        public static final double PID_TOLERANCE = 0.1;
+        public static final double PID_TOLERANCE = 0.3;
         public static final double MIN_PERCENT_MANUAL = -0.15;
         public static final double MAX_PERCENT_MANUAL = 0.15;
 

diff --git a/src/main/java/team/gif/robot/commands/shooter/AutoDriveAndShoot.java b/src/main/java/team/gif/robot/commands/shooter/AutoDriveAndShoot.java
@@ -52,7 +52,7 @@ public void execute() {
     // Return true when the command should end, false if it should continue. Runs every ~20ms.
     @Override
     public boolean isFinished() {
-        return hasTarget;
+        return Robot.elevator.isReadyToShoot() && hasTarget;
     }
 
     // Called when the command ends or is interrupted.
@@ -62,7 +62,7 @@ public void end(boolean interrupted) {
         Robot.swerveDrive.setDrivePace(drivePace.COAST_FR);
 
         // only shoot if the robot found the target during the command
-        if (hasTarget) {
+        if (Robot.elevator.isReadyToShoot() && hasTarget) {
             // run the shooter using the standard shoot command and return the elevator
             new SequentialCommandGroup(
                     new Shoot(),

diff --git a/src/main/java/team/gif/robot/commands/shooter/AutonStrafeToTarget.java b/src/main/java/team/gif/robot/commands/shooter/AutonStrafeToTarget.java
@@ -28,7 +28,7 @@ public void execute() {
         // This makes sure the sensors don't trip if the robot lined up
         // correctly and is still raising the elevator
         // Since this only runs in auto, safe to just chcek height of level 4
-        if (Robot.elevator.getPosition() < Constants.Elevator.LEVEL_4_POSITION - 0.3 ) {
+        if (Robot.elevator.getPosition() < Constants.Elevator.LEVEL_4_POSITION - Constants.Elevator.PID_TOLERANCE) {
             return;
         }
 

diff --git a/src/main/java/team/gif/robot/subsystems/Elevator.java b/src/main/java/team/gif/robot/subsystems/Elevator.java
@@ -204,6 +204,20 @@ public boolean isMotionMagicFinished() {
         return Math.abs(PIDError()) < Constants.Elevator.PID_TOLERANCE;
     }
 
+    /**
+     * Determines if the elevator is up and at its target position. Will return false if
+     * the elevator is all the way down at the collector position
+     *
+     * @return true if the elevator is up ready to shoot, false if not
+     */
+    public boolean isReadyToShoot() {
+        if(getTargetPosition() == Constants.Elevator.COLLECTOR_POSITION) {
+            return false;
+        }
+
+        return Math.abs(getTargetPosition() - getPosition()) < Constants.Elevator.PID_TOLERANCE;
+    }
+
     /**
      * gets the motor output voltage
      *