From 3e6fa986e77ba29bd275e289e30a6308307ce016 Mon Sep 17 00:00:00 2001
From: Team 2648 <team2648@coldlightalchemist.com>
Date: Thu, 20 Mar 2025 19:00:37 -0400
Subject: [PATCH] Worked on the barge shot, added a consistent coral intake

---
 src/main/java/frc/robot/RobotContainer.java                 | 4 +++-
 src/main/java/frc/robot/constants/ManipulatorConstants.java | 6 ++++++
 .../java/frc/robot/constants/ManipulatorPivotConstants.java | 2 +-
 src/main/java/frc/robot/subsystems/Manipulator.java         | 3 ++-
 4 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1e16628..e5205c9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -143,6 +143,8 @@ public class RobotContainer {
 
 		driver.leftTrigger().whileTrue( 
 			manipulator.runUntilCollected(() -> 0.75)
+			.until(() -> manipulator.getCoralBeamBreak() == false)
+			.andThen(manipulator.retractCommand(() -> .1))
 		);
 
 		driver.start().and(driver.back()).onTrue(
@@ -495,7 +497,7 @@ public class RobotContainer {
 	private Command shootAlgae(){
 		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
 		.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>30).andThen(manipulator.runManipulator(() -> -1, false),
+		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>44).andThen(manipulator.runManipulator(() -> -1, false),
 		elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
 		.raceWith(elevator.maintainPosition()));
 	}
diff --git a/src/main/java/frc/robot/constants/ManipulatorConstants.java b/src/main/java/frc/robot/constants/ManipulatorConstants.java
index 5e4a580..c313c1d 100644
--- a/src/main/java/frc/robot/constants/ManipulatorConstants.java
+++ b/src/main/java/frc/robot/constants/ManipulatorConstants.java
@@ -1,5 +1,6 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ManipulatorConstants {
@@ -7,4 +8,9 @@ public class ManipulatorConstants {
     public static final int kCoralBeamBreakID = 2;
     
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+    
+    static{
+        motorConfig.smartCurrentLimit(40)
+        .idleMode(IdleMode.kBrake);
+    };
 }
diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
index b28d6e6..acf8d13 100644
--- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
+++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
@@ -20,7 +20,7 @@ public class ManipulatorPivotConstants {
 
     public static final double kPivotMaxVelocity = 2 * Math.PI;
 
-    public static final double kPositionalP = 4;
+    public static final double kPositionalP = 4.5;
     public static final double kPositionalI = 0;
     public static final double kPositionalD = 0;
     public static final double kPositionalTolerance = Units.degreesToRadians(1.5);
diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java
index 9f59550..8cbb6ed 100644
--- a/src/main/java/frc/robot/subsystems/Manipulator.java
+++ b/src/main/java/frc/robot/subsystems/Manipulator.java
@@ -71,7 +71,8 @@ public class Manipulator extends SubsystemBase {
             manipulatorMotor.set(
                 speed.getAsDouble()
             );
-        }).until(() -> !coralBeamBreak.get());
+        }).unless(() -> !coralBeamBreak.get())
+        .until(() -> !coralBeamBreak.get());
     }
 
     public Command retractCommand(DoubleSupplier retractSpeed){