squeeze/toerque retract

src/main/deploy/pathplanner/paths/New Path.path

@@ -0,0 +1,155 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.61204761904762,
+        "y": 1.918666666666668
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.5040714285714296,
+        "y": 1.4003809523809525
+      },
+      "isLocked": false,
+      "linkedName": "end of RIght trench first swipe"
+    },
+    {
+      "anchor": {
+        "x": 3.648619047619049,
+        "y": 0.6121547619047625
+      },
+      "prevControl": {
+        "x": 3.4190610119047635,
+        "y": 0.7741190476190478
+      },
+      "nextControl": {
+        "x": 3.878177083333335,
+        "y": 0.45019047619047714
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.333047619047619,
+        "y": 0.6121547619047625
+      },
+      "prevControl": {
+        "x": 4.4368452380952395,
+        "y": 0.6229523809523821
+      },
+      "nextControl": {
+        "x": 6.519422842432679,
+        "y": 0.5978610845145801
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.391214285714287,
+        "y": 3.7434642857142855
+      },
+      "prevControl": {
+        "x": 5.376238095238096,
+        "y": 2.7932738095238094
+      },
+      "nextControl": {
+        "x": 6.987633102739957,
+        "y": 4.301813816546828
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.589750000000002,
+        "y": 1.2492142857142858
+      },
+      "prevControl": {
+        "x": 7.488215383486641,
+        "y": 1.4776671728693485
+      },
+      "nextControl": {
+        "x": 7.8057023809523844,
+        "y": 0.7633214285714278
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.648619047619049,
+        "y": 0.4717857142857145
+      },
+      "prevControl": {
+        "x": 4.501280949513,
+        "y": 0.5293980049542248
+      },
+      "nextControl": {
+        "x": 2.8495952380952403,
+        "y": 0.41779761904761914
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.7374404761904771,
+        "y": 0.9792738095238098
+      },
+      "prevControl": {
+        "x": 2.4068928571428576,
+        "y": 1.3032023809523823
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 0.7377593360995809,
+      "rotationDegrees": -1.8097734110142742
+    },
+    {
+      "waypointRelativePos": 1.8273858921161648,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 2.364315352697095,
+      "rotationDegrees": 87.9267492938311
+    },
+    {
+      "waypointRelativePos": 3.5701244813277846,
+      "rotationDegrees": -59.49633403888164
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -132.7688253919687
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -133.33166255170664
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RightTrench-FirstSwipe.path

@@ -0,0 +1,127 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.8969642857142865,
+        "y": 0.6757407407407425
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.1494880952380955,
+        "y": 0.6013571428571436
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.237607142857144,
+        "y": 1.4759642857142854
+      },
+      "prevControl": {
+        "x": 7.558649163266191,
+        "y": 0.2980245529994656
+      },
+      "nextControl": {
+        "x": 8.584042208300819,
+        "y": 2.0770024068622486
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.237607142857144,
+        "y": 3.8190476190476197
+      },
+      "prevControl": {
+        "x": 8.310074297364315,
+        "y": 3.5787466468913385
+      },
+      "nextControl": {
+        "x": 8.165425863946698,
+        "y": 4.058400628167732
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.237607142857144,
+        "y": 1.0872500000000003
+      },
+      "prevControl": {
+        "x": 9.520930827264253,
+        "y": 2.0312275985936155
+      },
+      "nextControl": {
+        "x": 7.304441714310592,
+        "y": 0.40083918172158384
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.4434642857142865,
+        "y": 0.6757407407407425
+      },
+      "prevControl": {
+        "x": 4.9917133086412,
+        "y": 0.5083004760390194
+      },
+      "nextControl": {
+        "x": 2.6336428571428576,
+        "y": 0.7633214285714289
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.61204761904762,
+        "y": 1.918666666666668
+      },
+      "prevControl": {
+        "x": 2.502511133859423,
+        "y": 1.6751404175218187
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "end of RIght trench first swipe"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.9709543568464838,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 3.4315352697095514,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -133.33166255170664
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.7627685672325
+  },
+  "useDefaultConstraints": false
+}

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -5,15 +5,15 @@ package frc4388.robot.constants;
  */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "2026KPopRobotHunters-new";
+  public static final String MAVEN_NAME = "2026KPopRobotHunters";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 189;
+  public static final int GIT_REVISION = 190;
-  public static final String GIT_SHA = "65c76aca95de81f342df7ff26a77e18856f22a83";
+  public static final String GIT_SHA = "3fe5d0e5a1cfe24710b755b489fa61255e307095";
-  public static final String GIT_DATE = "2026-03-28 15:56:54 MDT";
+  public static final String GIT_DATE = "2026-03-30 16:15:43 MDT";
   public static final String GIT_BRANCH = "New-Intake";
-  public static final String BUILD_DATE = "2026-03-30 15:59:11 MDT";
+  public static final String BUILD_DATE = "2026-03-31 18:51:12 MDT";
-  public static final long BUILD_UNIX_TIME = 1774907951058L;
+  public static final long BUILD_UNIX_TIME = 1775004672077L;
-  public static final int DIRTY = 0;
+  public static final int DIRTY = 1;
 
   private BuildConstants(){}
 }

src/main/java/frc4388/robot/subsystems/intake/Intake.java

@@ -37,6 +37,7 @@ public class Intake extends SubsystemBase {
         Retracting,
 
         Idle,
+        RectractTorque,
         Bouncing
     }
 
@@ -85,6 +86,8 @@ public class Intake extends SubsystemBase {
     //     return FieldZone.InShootZone;
     // }
 
+
+
     @Override
     public void periodic() {
         // FaultReporter.register(this); // TODO Implement fault reporter
@@ -94,6 +97,7 @@ public class Intake extends SubsystemBase {
         Logger.processInputs("Intake", state);
         Logger.recordOutput("Intake/IntakeState", this.mode);
 
+
         io.updateInputs(state);
 
         // getCurrentTime
@@ -147,6 +151,19 @@ public class Intake extends SubsystemBase {
 
                 io.armOutput(percentOutput);
 
+                if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) {
+                    io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
+                } else {
+                    io.setRollerOutput(state, 0);
+                }
+                break;
+            case RectractTorque:
+                io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
+                if (state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_SQUEEZE_CURRENT_LIMIT.get()){
+                    io.armOutput(IntakeConstants.ARM_SQUEEZE_PERCENT_OUTPUT.get());
+                } else {
+                    io.armOutput(IntakeConstants.ARM_REDUCED_SQUEEZE_PERCENT_OUTPUT.get());
+                }
                 if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) {
                     io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
                 } else {

src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java

@@ -24,6 +24,11 @@ public class IntakeConstants {
     public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 20);
 
 
+    //squeeze constants
+    public static final ConfigurableDouble INTAKE_SQUEEZE_CURRENT_LIMIT = new ConfigurableDouble("Intake Squeeze Current Limit", 20);
+    public static final ConfigurableDouble ARM_SQUEEZE_PERCENT_OUTPUT = new ConfigurableDouble("Arm squeeze % output", -0.1);
+    public static final ConfigurableDouble ARM_REDUCED_SQUEEZE_PERCENT_OUTPUT = new ConfigurableDouble("Arm squeeze % output", -0.02);
+
 
     //IDs