Merge branch 'New-Intake' into DenverAutos

elastic-layout.json

@@ -8,10 +8,10 @@
         "layouts": [
           {
             "title": "Tag Processed",
-            "x": 896.0,
+            "x": 384.0,
             "y": 0.0,
             "width": 256.0,
-            "height": 384.0,
+            "height": 256.0,
             "type": "List Layout",
             "properties": {
               "label_position": "TOP"
@@ -56,10 +56,26 @@
         ],
         "containers": [
           {
-            "title": "RetractedLimit",
+            "title": "MatchTime",
-            "x": 512.0,
+            "x": 0.0,
             "y": 0.0,
-            "width": 256.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "Match Time",
+            "properties": {
+              "topic": "/AdvantageKit/DriverStation/MatchTime",
+              "period": 0.06,
+              "data_type": "double",
+              "time_display_mode": "Minutes and Seconds",
+              "red_start_time": 15,
+              "yellow_start_time": 30
+            }
+          },
+          {
+            "title": "RetractedLimit",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 128.0,
             "height": 128.0,
             "type": "Boolean Box",
             "properties": {
@@ -74,9 +90,9 @@
           },
           {
             "title": "Auto Chooser",
-            "x": 896.0,
+            "x": 1024.0,
-            "y": 384.0,
+            "y": 256.0,
-            "width": 256.0,
+            "width": 384.0,
             "height": 128.0,
             "type": "ComboBox Chooser",
             "properties": {
@@ -87,9 +103,9 @@
           },
           {
             "title": "Roller Percent Output",
-            "x": 512.0,
+            "x": 0.0,
             "y": 256.0,
-            "width": 384.0,
+            "width": 256.0,
             "height": 128.0,
             "type": "Number Slider",
             "properties": {
@@ -104,8 +120,8 @@
           },
           {
             "title": "Shooter OVERRIDE Velocity",
-            "x": 768.0,
+            "x": 128.0,
-            "y": 0.0,
+            "y": 384.0,
             "width": 128.0,
             "height": 128.0,
             "type": "Text Display",
@@ -118,8 +134,8 @@
           },
           {
             "title": "TRIM SHOOTER SPEED",
-            "x": 512.0,
+            "x": 256.0,
-            "y": 384.0,
+            "y": 256.0,
             "width": 384.0,
             "height": 128.0,
             "type": "Number Slider",
@@ -135,8 +151,8 @@
           },
           {
             "title": "Mode",
-            "x": 512.0,
+            "x": 256.0,
-            "y": 128.0,
+            "y": 384.0,
             "width": 384.0,
             "height": 128.0,
             "type": "Text Display",
@@ -148,49 +164,58 @@
             }
           },
           {
-            "title": "IsActive",
+            "title": "Time to rev",
-            "x": 0.0,
+            "x": 768.0,
-            "y": 0.0,
+            "y": 128.0,
-            "width": 384.0,
+            "width": 256.0,
-            "height": 256.0,
-            "type": "Boolean Box",
-            "properties": {
-              "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive",
-              "period": 0.06,
-              "data_type": "boolean",
-              "true_color": 4283215696,
-              "false_color": 4294198070,
-              "true_icon": "None",
-              "false_icon": "None"
-            }
-          },
-          {
-            "title": "RemainingInShift",
-            "x": 0.0,
-            "y": 256.0,
-            "width": 384.0,
-            "height": 128.0,
-            "type": "Match Time",
-            "properties": {
-              "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift",
-              "period": 0.06,
-              "data_type": "double",
-              "time_display_mode": "Minutes and Seconds",
-              "red_start_time": 15,
-              "yellow_start_time": 30
-            }
-          },
-          {
-            "title": "Phase",
-            "x": 0.0,
-            "y": 384.0,
-            "width": 384.0,
             "height": 128.0,
             "type": "Large Text Display",
             "properties": {
-              "topic": "/AdvantageKit/RealOutputs/HubShift/Phase",
+              "topic": "/AdvantageKit/RealOutputs/Time to rev",
               "period": 0.06,
-              "data_type": "string"
+              "data_type": "double"
+            }
+          },
+          {
+            "title": "Arm angle extended",
+            "x": 1152.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Arm angle extended",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Current",
+            "x": 640.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Current",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Position",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Position",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
             }
           }
         ]
@@ -530,8 +555,8 @@
           },
           {
             "title": "Shooter Idle max current",
-            "x": 1024.0,
+            "x": 384.0,
-            "y": 384.0,
+            "y": 512.0,
             "width": 256.0,
             "height": 128.0,
             "type": "Text Display",
@@ -558,8 +583,8 @@
           },
           {
             "title": "Shooter OVERRIDE Velocity",
-            "x": 1024.0,
+            "x": 640.0,
-            "y": 256.0,
+            "y": 512.0,
             "width": 256.0,
             "height": 128.0,
             "type": "Text Display",
@@ -578,40 +603,12 @@
       "grid_layout": {
         "layouts": [],
         "containers": [
-          {
-            "title": "Stalled Motor: ",
-            "x": 512.0,
-            "y": 128.0,
-            "width": 384.0,
-            "height": 128.0,
-            "type": "Text Display",
-            "properties": {
-              "topic": "/AdvantageKit/RealOutputs/Stalled Motor: ",
-              "period": 0.06,
-              "data_type": "string",
-              "show_submit_button": false
-            }
-          },
-          {
-            "title": "Shooter idle % output",
-            "x": 128.0,
-            "y": 256.0,
-            "width": 256.0,
-            "height": 128.0,
-            "type": "Text Display",
-            "properties": {
-              "topic": "/SmartDashboard/Shooter idle % output",
-              "period": 0.06,
-              "data_type": "double",
-              "show_submit_button": true
-            }
-          },
           {
             "title": "Auto Chooser",
-            "x": 512.0,
+            "x": 0.0,
-            "y": 256.0,
+            "y": 0.0,
-            "width": 384.0,
+            "width": 640.0,
-            "height": 128.0,
+            "height": 768.0,
             "type": "ComboBox Chooser",
             "properties": {
               "topic": "/SmartDashboard/Auto Chooser",
@@ -621,6 +618,275 @@
           }
         ]
       }
+    },
+    {
+      "name": "ARM",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "IntakeState",
+            "x": 640.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Intake/IntakeState",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "ArmAngle",
+            "x": 256.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/ArmAngle",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "IntakeEncoder",
+            "x": 256.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/IntakeEncoder",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "RetractedLimitSwitch",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/RetractedLimitSwitch",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Roller Retract Output",
+            "x": 384.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Roller Retract Output",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Roller Percent Output",
+            "x": 384.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Roller Percent Output",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Arm extend % output",
+            "x": 384.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Arm extend % output",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Arm retract % output",
+            "x": 384.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Arm retract % output",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "RetractedSoftLimit",
+            "x": 0.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/RetractedSoftLimit",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "ExtendedSoftLimit",
+            "x": 0.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/ExtendedSoftLimit",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Arm reverse roller range",
+            "x": 640.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Arm reverse roller range",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Arm angle extended",
+            "x": 640.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Arm angle extended",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Arm Encoder Offset",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Arm Encoder Offset",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Bounce Half Period",
+            "x": 896.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Bounce Half Period",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Bounce Max Output",
+            "x": 896.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Bounce Max Output",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Bounce Output",
+            "x": 896.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Bounce Output",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Intake Bounce Current Limit",
+            "x": 896.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Intake Bounce Current Limit",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "Intake Bounce Velocity Limit",
+            "x": 640.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Intake Bounce Velocity Limit",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          }
+        ]
+      }
     }
   ]
 }

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/RobotContainer.java

@@ -404,7 +404,7 @@ public class RobotContainer {
             
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
             .onTrue(new InstantCommand(() -> {
-                m_robotIntake.setMode(IntakeMode.Bouncing);
+                m_robotIntake.setMode(IntakeMode.RectractTorque);
             }))
             .onFalse(new InstantCommand(() -> {
                 m_robotIntake.setMode(IntakeMode.Idle);

src/main/java/frc4388/robot/RobotMap.java

@@ -101,7 +101,7 @@ public class RobotMap {
                 
                 //Configure Intake 20,21
                 SparkMax arm = new SparkMax(IntakeConstants.ARM_ID.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless);
-                SparkMax roller = new SparkMax(IntakeConstants.ROLLER_ID.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless);
+                TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.CANIVORE_CANBUS);
                 // DigitalInput armLimitSwitch = new DigitalInput(IntakeConstants.ARM_LIMIT_SWITCH_CHANNEL);
                 // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
                 // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
@@ -120,7 +120,7 @@ public class RobotMap {
                 FaultTalonFX.addDevice(shooter2, "Shooter2");
                 FaultTalonFX.addDevice(indexer, "Indexer");
                 FaultSparkMax.addDevice(arm, "Arm");
-                FaultSparkMax.addDevice(roller, "Roller");
+                FaultTalonFX.addDevice(roller, "Roller");
                 
                 FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
                 FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");

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

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2026KPopRobotHunters";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 191;
+  public static final int GIT_REVISION = 198;
-  public static final String GIT_SHA = "9021f480beaf35912eb54a845ec87522fef5337a";
+  public static final String GIT_SHA = "958bdc46fd90b8388750f8bf1a95b62ff91b2092";
-  public static final String GIT_DATE = "2026-03-30 18:57:14 MDT";
+  public static final String GIT_DATE = "2026-03-31 20:51:47 MDT";
   public static final String GIT_BRANCH = "New-Intake";
-  public static final String BUILD_DATE = "2026-03-30 19:50:05 MDT";
+  public static final String BUILD_DATE = "2026-04-01 17:29:06 MDT";
-  public static final long BUILD_UNIX_TIME = 1774921805228L;
+  public static final long BUILD_UNIX_TIME = 1775086146983L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -37,8 +37,10 @@ public class Intake extends SubsystemBase {
         Retracting,
 
         Idle,
+        RectractTorque,
         Bouncing
     }
+    private boolean overCompressed = false;
 
     private IntakeMode mode = IntakeMode.Idle;
 
@@ -85,6 +87,7 @@ public class Intake extends SubsystemBase {
     //     return FieldZone.InShootZone;
     // }
 
+
     @Override
     public void periodic() {
         // FaultReporter.register(this); // TODO Implement fault reporter
@@ -94,8 +97,17 @@ public class Intake extends SubsystemBase {
         Logger.processInputs("Intake", state);
         Logger.recordOutput("Intake/IntakeState", this.mode);
 
+
         io.updateInputs(state);
 
+        if (state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_SQUEEZE_CURRENT_LOWER_THRESHOLD.get()){
+            overCompressed = false;
+        } else if (state.armMotorCurrent.in(Amps) > IntakeConstants.INTAKE_SQUEEZE_CURRENT_UPPER_THRESHOLD.get()) {
+            overCompressed = true;
+        }
+
+        Logger.recordOutput("overCompressed", overCompressed);
+
         // getCurrentTime
 
         switch (mode) {
@@ -154,6 +166,19 @@ public class Intake extends SubsystemBase {
                     io.setRollerOutput(state, 0);
                 }
                 break;
+            case RectractTorque:
+                io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
+                if (!overCompressed){
+                    io.armOutput(IntakeConstants.ARM_SQUEEZE_PERCENT_OUTPUT.get());
+                } else if (overCompressed) {
+                    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 {
+                    io.setRollerOutput(state, 0);
+                }
+                break;
             case Idle:
                 io.armOutput(0);
                 io.setRollerOutput(state, 0);

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

@@ -1,5 +1,9 @@
 package frc4388.robot.subsystems.intake;
 
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.LimitSwitchConfig.Behavior;
@@ -25,6 +29,12 @@ public class IntakeConstants {
     public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 4);
 
 
+    //squeeze constants
+    public static final ConfigurableDouble INTAKE_SQUEEZE_CURRENT_LOWER_THRESHOLD = new ConfigurableDouble("Intake Squeeze Current LOWER THRESHOLD", 20);
+    public static final ConfigurableDouble INTAKE_SQUEEZE_CURRENT_UPPER_THRESHOLD = new ConfigurableDouble("Intake Squeeze Current UPPER THRESHOLD", 25);
+    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 reduce squeeze % output", -0.02);
+
 
     //IDs
 
@@ -43,7 +53,7 @@ public class IntakeConstants {
     // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
 
     public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.);
-    public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 1.5);
+    public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 1.75);
     public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.2);
     public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.2);
 
@@ -79,7 +89,7 @@ public class IntakeConstants {
     // Motor configs
 
     public static final SparkMaxConfig ARM_MOTOR_CONFIG = new SparkMaxConfig();
-    public static final SparkMaxConfig ROLELR_MOTOR_CONFIG = new SparkMaxConfig();
+    public static final CanDevice ROLLER_MOTOR_ID = new CanDevice("INTAKE_ROLLER", 21);
 
     static {
         ARM_MOTOR_CONFIG.limitSwitch
@@ -95,7 +105,7 @@ public class IntakeConstants {
         ARM_MOTOR_CONFIG.idleMode(IdleMode.kBrake);
 
         
-        ROLELR_MOTOR_CONFIG.idleMode(IdleMode.kCoast);
+        // ROLLER_MOTOR_CONFIG.idleMode(IdleMode.kCoast);
     }
 
     // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
@@ -109,14 +119,14 @@ public class IntakeConstants {
     //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
     // );
 
-    // public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration()
+    public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration()
-    //     .withCurrentLimits(
+        .withCurrentLimits(
-    //         new CurrentLimitsConfigs()
+            new CurrentLimitsConfigs()
-    //             .withStatorCurrentLimit(40) // TODO: tune???
+                .withStatorCurrentLimit(40) // TODO: tune???
-    //             .withStatorCurrentLimitEnable(true)
+                .withStatorCurrentLimitEnable(true)
-    //         ).withMotorOutput(
+            ).withMotorOutput(
-    //             new MotorOutputConfigs()
+                new MotorOutputConfigs()
-    //                 .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
+                    .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
-    //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
+                    .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
-    // );
+    );
 }

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

@@ -21,17 +21,18 @@ import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Velocity;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import frc4388.robot.subsystems.shooter.ShooterConstants;
 import frc4388.utility.compute.JankCoder;
 
 public class IntakeReal implements IntakeIO {
 
     SparkMax m_armMotor;
-    SparkMax m_rollerMotor;
+    TalonFX m_rollerMotor;
     JankCoder m_encoder;
 
     public IntakeReal(
         SparkMax armMotor,
-        SparkMax rollerMotor,
+        TalonFX rollerMotor,
         JankCoder jankCoder
     ) {
         // m_angleMotor = angleMotor;
@@ -41,7 +42,7 @@ public class IntakeReal implements IntakeIO {
         m_encoder = jankCoder;
 
         m_armMotor.configure(IntakeConstants.ARM_MOTOR_CONFIG, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
-        m_rollerMotor.configure(IntakeConstants.ROLELR_MOTOR_CONFIG, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
+        m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG);
     }
 
 
@@ -113,7 +114,7 @@ public class IntakeReal implements IntakeIO {
         state.armMotorCurrent = Amps.of(m_armMotor.getOutputCurrent());
 
         state.rollerOutput = m_rollerMotor.get();
-        state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent());
+        // state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent());
 
         state.retractedSoftLimit = retractedLimit();
         state.extendedSoftLimit = extendedLimit();

src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java

@@ -67,16 +67,16 @@ public final class SwerveDriveConstants {
 
     // Operation
     public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
+    public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05); // degrees to add to the angle when we are far away, to account for camera misalignment. TODO: find value.
 
     public static final boolean DRIFT_CORRECTION_ENABLED = true;
     public static final boolean INVERT_X = false;
     public static final boolean INVERT_Y = true;
     public static final boolean INVERT_ROTATION = false;
 
-    public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05);
     // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
 
-    public static final class ModuleSpecificConstants { //2026
+    private static final class ModuleSpecificConstants { //2025
         //Front Left
         private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
         private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
@@ -166,6 +166,7 @@ public final class SwerveDriveConstants {
         public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 12);
         public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0);
         public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1);
+        public static final ConfigurableDouble HOLD_POSITION_kP = new ConfigurableDouble("Hold Position kP", 15);
         // public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1);
     }