added hasGamePiece boolean logic (hopefully good)

Patribots4738 · Jan 25, 2024 · bf1f07a · bf1f07a · Jacob1010-h · Jan 25, 2024
1 parent 1990c43
commit bf1f07a
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 17 deletions.
diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java
@@ -25,8 +25,6 @@ public ShooterCalc(Shooter shooter, Pivot pivot) {
     this.aiming = false;
   }
 
-
-
   /**
    * The function prepares a fire command by calculating the speed and angle for the robot's shooter
    * based on the robot's pose and whether it should shoot at the speaker.

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubs/Claw.java b/src/main/java/frc/robot/subsystems/ElevatorSubs/Claw.java
@@ -9,13 +9,14 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.DriverUI;
 import frc.robot.util.Neo;
 import frc.robot.util.Constants.TrapConstants;
 
 public class Claw extends SubsystemBase {
     private final Neo claw;
     private boolean hasGamePiece = false;
-    private boolean hadGamePieceLast = false;
+    private double current = 0;
     private double startIntakingTimestamp = 0;
 
     /** Creates a new Claw. */
@@ -26,8 +27,8 @@ public Claw() {
 
     @Override
     public void periodic() {
-        this.hadGamePieceLast = this.hasGamePiece;
         this.hasGamePiece = hasGamePiece();
+        updateOutputCurrent();
     }
 
     public Command placeCommand() {
@@ -48,23 +49,23 @@ public Command intakeFromHandoff() {
     }
 
     public boolean hasGamePiece() {
-        // TODO: needs logic
-
         // if appliedoutput <= desired/2 and 
         // current this loop and last loop > constant
         // desired speed > constant 
         // we've been intaking over .25 seconds (make constant);
-        // if (hasGamePiece) {
-        //     this.hasGamePiece = claw.getTargetVelocity() > 0;
-        // }
-        // else {
-        //     this.hasGamePiece = 
-        //         (-0.25 < claw.getAppliedOutput() && claw.getAppliedOutput() < 0) && 
-        //         (current > 15 && claw.getOutputCurrent() > 15) &&
-        //         (desiredSpeed > 0.45) &&
-        //         (DriverUI.currentTimestamp - startedIntakingTimestamp > 0.25);
-        // }
-        return false;
+        if (hasGamePiece) {
+            this.hasGamePiece = claw.getTargetVelocity() > 0;
+        }
+        else {
+            this.hasGamePiece = 
+                (TrapConstants.CLAW_HAS_PIECE_LOWER_LIMIT < claw.getAppliedOutput() && 
+                    claw.getAppliedOutput() < TrapConstants.CLAW_HAS_PIECE_UPPER_LIMIT) && 
+                (current > TrapConstants.CLAW_HAS_PIECE_MIN_CURRENT && 
+                    claw.getOutputCurrent() > TrapConstants.CLAW_HAS_PIECE_MIN_CURRENT) &&
+                (claw.getTargetVelocity() > TrapConstants.CLAW_HAS_PIECE_MIN_TARGET_VELO) &&
+                (DriverUI.currentTimestamp - startIntakingTimestamp > TrapConstants.CLAW_HAS_PIECE_MIN_TIMESTAMP);
+        }
+        return hasGamePiece;
     }
 
     public void configMotors() {
@@ -76,6 +77,10 @@ public void configMotors() {
         claw.setBrakeMode();
     }
 
+    public void updateOutputCurrent() {
+        current = claw.getOutputCurrent();
+    }
+
     public boolean getHasGamePiece() {
         return this.hasGamePiece;
     }
@@ -89,6 +94,7 @@ public Command stop() {
     }
 
     public Command intake() {
+        startIntakingTimestamp = DriverUI.currentTimestamp;
         return runOnce(() -> claw.setTargetVelocity(TrapConstants.CLAW_INTAKE));
     }
 

diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java
@@ -159,6 +159,15 @@ public static final class TrapConstants {
                 public static final double CLAW_OUTTAKE = 0;
                 public static final double CLAW_INTAKE = 0;
                 public static final double TRAP_PLACE_POS = 0;
+
+                public static final double CLAW_HAS_PIECE_UPPER_LIMIT = 0;
+                public static final double CLAW_HAS_PIECE_LOWER_LIMIT = -0.25;
+
+                public static final double CLAW_HAS_PIECE_MIN_TIMESTAMP = 0.25;
+
+                public static final double CLAW_HAS_PIECE_MIN_CURRENT = 15;
+
+                public static final double CLAW_HAS_PIECE_MIN_TARGET_VELO = 0.45;
         }
 
         public static final class ClimbConstants {