diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index a4c9cc99..ca9cd0bf 100644 --- 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 index bffb54a1..c2ce4870 100644 --- 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 index e2e18317..82555023 100644 --- 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 {