Adding trench autos and linear model for roller

src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto

@@ -39,6 +39,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "Spring Break Tests",
+  "folder": null,
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/ShootPreloadBump.auto

@@ -38,6 +38,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "Spring Break Tests",
+  "folder": null,
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto

@@ -0,0 +1,55 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftTrench-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto

@@ -0,0 +1,49 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightTrench-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path

@@ -0,0 +1,152 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5373205128205143,
+        "y": 7.430435897435898
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7870116206619695,
+        "y": 7.418012103131955
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.642076923076923,
+        "y": 7.069961538461539
+      },
+      "prevControl": {
+        "x": 7.178228149431453,
+        "y": 7.5205380189223625
+      },
+      "nextControl": {
+        "x": 8.216115793872147,
+        "y": 6.51234787481454
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.781615384615385,
+        "y": 4.837346153846154
+      },
+      "prevControl": {
+        "x": 7.786585532087434,
+        "y": 5.241663699788379
+      },
+      "nextControl": {
+        "x": 7.778542446002628,
+        "y": 4.5873650404629975
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.572282051282052,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 6.957648225000392,
+        "y": 5.601152104386376
+      },
+      "nextControl": {
+        "x": 6.204516600433754,
+        "y": 5.426493771942126
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 3.7826390804558825,
+        "y": 5.585258005415613
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": 112.5
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 2.374307862679942,
+      "maxWaypointRelativePos": 4.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 10.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.9302325581395282,
+      "maxWaypointRelativePos": 2.1705426356589252,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 0.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 2.4983388704318887,
+      "maxWaypointRelativePos": 3.2956810631229274,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 7.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.7353266888150656,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 101.70667085951649
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path

@@ -0,0 +1,165 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6070897435897447,
+        "y": 0.6046794871794885
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8833980386504074,
+        "y": 0.5273348158056345
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.595564102564104,
+        "y": 0.9884102564102579
+      },
+      "prevControl": {
+        "x": 7.151835398129718,
+        "y": 0.5429530943613806
+      },
+      "nextControl": {
+        "x": 7.93843114627953,
+        "y": 1.332612871042205
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.793243589743589,
+        "y": 3.4070769230769233
+      },
+      "prevControl": {
+        "x": 7.74538518514461,
+        "y": 3.9526634447090423
+      },
+      "nextControl": {
+        "x": 7.8344917146104915,
+        "y": 2.936847688346859
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.572282051282052,
+        "y": 2.5119999999999996
+      },
+      "prevControl": {
+        "x": 7.050706524183416,
+        "y": 2.443033938208306
+      },
+      "nextControl": {
+        "x": 6.24953920807272,
+        "y": 2.5585241728388555
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 2.512
+      },
+      "prevControl": {
+        "x": 3.841498071206661,
+        "y": 2.6514755211571583
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": -112.5
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 2.2059800664451794,
+      "maxWaypointRelativePos": 4.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 10.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.771871539313429,
+      "maxWaypointRelativePos": 2.542635658914713,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 6.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.859357696566985,
+      "maxWaypointRelativePos": 2.1705426356589252,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 2.4983388704318887,
+      "maxWaypointRelativePos": 3.2956810631229274,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.6
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.6910299003322272,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": -113.07
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.3,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/java/frc4388/robot/RobotContainer.java

@@ -81,7 +81,7 @@ public class RobotContainer {
     //Testing of Colors
     public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
-    public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO);
+    public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO, m_robotSwerveDrive);
     public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
     
 
@@ -109,7 +109,7 @@ public class RobotContainer {
     
     
         private Command IntakeExtended = new SequentialCommandGroup(
-            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendedREMOVEME), m_robotIntake)
+            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendingRolling), m_robotIntake)
         );
     
         // private Command LidarIntake = new SequentialCommandGroup(
@@ -146,7 +146,7 @@ public class RobotContainer {
         );
     
         private Command IntakeRetracted = new SequentialCommandGroup(
-            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RetractedREMOVEME), m_robotIntake)
+            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RectractTorque), m_robotIntake)
         );
     
         private Command RobotShoot = new SequentialCommandGroup(

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

@@ -2,7 +2,6 @@ package frc4388.robot.subsystems.intake;
 
 import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.Rotations;
-import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import java.util.function.Supplier;
 
@@ -11,19 +10,23 @@ import org.littletonrobotics.junction.Logger;
 import com.ctre.phoenix6.Utils;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 public class Intake extends SubsystemBase {
     public IntakeIO io;
     IntakeStateAutoLogged state = new IntakeStateAutoLogged();
-
+    SwerveDrive m_SwerveDrive;
     Supplier<Pose2d> m_swervePoseSupplier;
 
     public Intake(
-        IntakeIO io
+        IntakeIO io,
+        SwerveDrive m_SwerveDrive
         // Supplier<Pose2d> swervePoseSupplier
     ) {
         this.io = io;
+        this.m_SwerveDrive = m_SwerveDrive;
         // this.m_swervePoseSupplier = swervePoseSupplier;
     }
 
@@ -92,8 +95,9 @@ public class Intake extends SubsystemBase {
     public void periodic() {
         // FaultReporter.register(this); // TODO Implement fault reporter
         // System.out.println(m_armLimitSwitch.get());
+        ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
 
-
+        double ChassisOverallSpeed = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond);
         Logger.processInputs("Intake", state);
         Logger.recordOutput("Intake/IntakeState", this.mode);
 
@@ -127,7 +131,7 @@ public class Intake extends SubsystemBase {
                 
             case ExtendingRolling:
                 io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
-                io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
+                io.setRollerOutput(state, IntakeConstants. ROLLER_PERCENT_OUTPUT.get()); //getTargetRollerSpeed(ChassisOverallSpeed));
                 break;
 
             case Retracting:

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

@@ -6,6 +6,9 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
+
+import edu.wpi.first.units.measure.AngularVelocity;
+
 import com.revrobotics.spark.config.LimitSwitchConfig.Behavior;
 import com.revrobotics.spark.config.LimitSwitchConfig.Type;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -59,8 +62,9 @@ public class IntakeConstants {
 
     public static final ConfigurableDouble ARM_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 1.17);
     
-    public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .80);
+    public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .50);
     public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .40);
+    public static final ConfigurableDouble ROLLER_MULTIPLIER_CONST = new ConfigurableDouble("Roller Multiplier Constant", 0.4);
 
     // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
 
@@ -79,7 +83,9 @@ public class IntakeConstants {
     // public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
     // public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.05);
     
-
+    public static double getTargetRollerSpeed(double chassisSpeed) {
+        return ROLLER_PERCENT_OUTPUT.get() * (1 + ROLLER_MULTIPLIER_CONST.get() * chassisSpeed);
+    }
 
 
     // 0 is paralell to the ground, 90 is directly up