Merge pull request #359 from frc-862/356-collect-end

[#356] - end smart collect for chase pieces
frc-862 · Mar 2, 2024 · 12d14a3 · 12d14a3
2 parents c6b27f8 + 9144577
commit 12d14a3
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -447,7 +447,7 @@ public class PivotConstants { // TODO: get real
         public static final double BIAS_INCREMENT = 1d; // Degrees to bias by per button press TODO get amount to bias
                                                         // by
 
-        public static final double STOW_ANGLE = 30d;
+        public static final double STOW_ANGLE = 28d;
 
         public static final double MAX_INDEX_ANGLE = 40d;
 

diff --git a/src/main/java/frc/robot/command/ChasePieces.java b/src/main/java/frc/robot/command/ChasePieces.java
@@ -31,6 +31,7 @@ public class ChasePieces extends Command {
 
     private boolean onTarget;
 	private boolean hasPiece;
+	private boolean isDone;
 	private boolean hasTarget;
 
 	private Command smartCollect;
@@ -70,6 +71,8 @@ public void initialize() {
 	private void initLogging() {
 		LightningShuffleboard.setBoolSupplier("ChasePieces", "On Target", () -> onTarget);
 		LightningShuffleboard.setBoolSupplier("ChasePieces", "Has Target", () -> hasTarget);
+		LightningShuffleboard.setBoolSupplier("ChasePieces", "Is Done", () -> isDone);
+		LightningShuffleboard.setBoolSupplier("ChasePieces", "Has Piece", () -> hasPiece);
 
 		LightningShuffleboard.setDoubleSupplier("ChasePieces", "Target Heading", () -> targetHeading);
 		LightningShuffleboard.setDoubleSupplier("ChasePieces", "Target Y", () -> targetPitch);
@@ -88,14 +91,15 @@ public void execute() {
 		}
 
 		onTarget = Math.abs(targetHeading) < VisionConstants.ALIGNMENT_TOLERANCE;
-		hasPiece = debouncer.calculate(indexer.getExitBeamBreakState());
+		isDone = smartCollect.isFinished();
+		hasPiece = debouncer.calculate(indexer.getEntryBeamBreakState()) || collector.getEntryBeamBreakState();
 
 		pidOutput = headingController.calculate(0, targetHeading);
 
 		if (!hasPiece){
 			if (hasTarget){
 				if (trustValues()){
-					power = 0.75d;
+					power = 0.65d;
 					if (!onTarget) {
 						drivetrain.setRobot(3, 0, -pidOutput);
 					} else {
@@ -105,12 +109,17 @@ public void execute() {
 			} else {
 				drivetrain.setRobot(3, 0, 0);
 			}
+		} else {
+			drivetrain.setRobot(0, 0, 0);
 		}
 
 	}
 
 	@Override
-	public void end(boolean interrupted) {}
+	public void end(boolean interrupted) {
+		power = 0d;
+		smartCollect.end(interrupted);
+	}
 
 	/**
 	 * Makes sure that the robot isn't jerking over to a different side while chasing pieces.
@@ -125,6 +134,6 @@ public boolean trustValues(){
 
 	@Override
 	public boolean isFinished() {
-		return hasPiece;
+		return isDone;
 	}
 }
diff --git a/src/main/java/frc/robot/command/SmartCollect.java b/src/main/java/frc/robot/command/SmartCollect.java
@@ -4,6 +4,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
+import frc.robot.Constants.IndexerConstants.PieceState;
 import frc.robot.subsystems.Collector;
 import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Pivot;
@@ -93,6 +94,9 @@ public void end(boolean interrupted) {
 
 	@Override
 	public boolean isFinished() {
+		if (reversedFromExit && indexer.getPieceState() == PieceState.IN_PIVOT){
+			return true;
+		}
 		return false;
 	}
 }
diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java
@@ -28,7 +28,7 @@ public class Pivot extends SubsystemBase {
     private final PIDController angleController = new PIDController(0.06, 0, 0);
     private double bias = 0;
 
-    private double targetAngle = 30;
+    private double targetAngle = PivotConstants.STOW_ANGLE;
 
     public Pivot() {
         CANcoderConfiguration angleConfig = new CANcoderConfiguration();