time commands for aiming pivot

grt192 · Feb 2, 2024 · dd0994d · dd0994d
1 parent 82e2188
commit dd0994d
Showing 1 changed file with 14 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java
@@ -8,6 +8,7 @@ public class ShooterPivotAimCommand extends Command{
 
     ShooterPivotSubsystem shooterPivotSubsystem;
     Timer timer;
+    private final double AIMING_TIME_TOLERANCE = 0.8;
 
     public ShooterPivotAimCommand(ShooterPivotSubsystem pivotSubsystem){
         this.shooterPivotSubsystem = pivotSubsystem;
@@ -23,7 +24,19 @@ public void end(){
     }
 
     public boolean isFinished(){
-        return(Math.abs(shooterPivotSubsystem.getPosition() - shooterPivotSubsystem.getAutoAimAngle()) < shooterPivotSubsystem.ERRORTOLERANCE);
+        boolean pivotInRange = Math.abs(shooterPivotSubsystem.getPosition() - shooterPivotSubsystem.getAutoAimAngle()) < shooterPivotSubsystem.ERRORTOLERANCE;
+        if(pivotInRange){
+            timer.start();
+        }
 
+        if(!pivotInRange){
+            timer.reset();
+        }
+
+        if(timer.hasElapsed(AIMING_TIME_TOLERANCE)){
+            return true;
+        } else {
+            return false;
+        }
     }
 }