auto paths and heading = 0

src/main/deploy/pathplanner/Five Ball.path

@@ -0,0 +1 @@
+{"waypoints":[{"anchorPoint":{"x":8.334720117716095,"y":1.881355486848905},"prevControl":null,"nextControl":{"x":8.334720117716095,"y":1.2691323148186409},"holonomicAngle":-90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.019409142266559,"y":0.26275914620794727},"prevControl":{"x":8.43030614852931,"y":0.2186359777502019},"nextControl":{"x":6.453364630867194,"y":0.430924999781035},"holonomicAngle":178.16716049405798,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.118548168124668,"y":1.9969695111773362},"prevControl":{"x":5.423348777725886,"y":1.3032853651883554},"nextControl":{"x":4.8562731024023424,"y":2.5938713848902157},"holonomicAngle":124.12854707414519,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.3768579261296454,"y":1.187671340859924},"prevControl":{"x":0.7357256093822531,"y":0.7147048776856182},"nextControl":{"x":2.0179902428770378,"y":1.6606378040342298},"holonomicAngle":-147.6118851465991,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.971403046254368,"y":6.06448109447943},"prevControl":{"x":3.647096949366312,"y":4.256698168568751},"nextControl":null,"holonomicAngle":47.68377515946898,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}

src/main/deploy/pathplanner/Move Down.path

@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":null,"nextControl":{"x":5.034465241344275,"y":5.002934143799322},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":4.992423777951004,"y":2.9954542667706034},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
+{"waypoints":[{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":null,"nextControl":{"x":5.034589912702322,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.991164682737528,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null}

src/main/deploy/pathplanner/Move Forward.path

@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":2.0390109745736735,"y":2.0},"prevControl":null,"nextControl":{"x":2.049521340421991,"y":1.9894896341516821},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.0,"y":2.0},"prevControl":{"x":6.996174239263382,"y":2.003825760736618},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null}
+{"waypoints":[{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":null,"nextControl":{"x":2.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null}

src/main/java/frc4388/robot/Constants.java

@@ -66,9 +66,9 @@ public final class Constants {
 
     // swerve auto constants
     public static final PIDController X_CONTROLLER = new PIDController(4.0, 0.0, 0.0);
-    public static final PIDController Y_CONTROLLER = new PIDController(4.0, 0.0, 0.0);
+    public static final PIDController Y_CONTROLLER = new PIDController(0.01, 0.0, 0.0);
     public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(
-      1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI));
+      1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
 
     // swerve configuration
     public static final double NEUTRAL_DEADBAND = 0.04;

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

@@ -181,7 +181,8 @@ public class RobotContainer {
     //   ppSwerveControllerCommand,
     //   new InstantCommand(() -> m_robotSwerveDrive.stopModules())
     // );
-    return runAuto("Move Forward", 5.0, 5.0);
+    // return runAuto("Move Forward", 5.0, 5.0);
+    return runAuto("Five Ball", 1.0, 1.0);
   }
 
   /**

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -49,9 +49,9 @@ public class SwerveModule extends SubsystemBase {
     angleMotor.configAllSettings(angleTalonFXConfiguration);
 
     TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
-    driveTalonFXConfiguration.slot0.kP = 1.0;
+    driveTalonFXConfiguration.slot0.kP = 0.15;
     driveTalonFXConfiguration.slot0.kI = 0.0;
-    driveTalonFXConfiguration.slot0.kD = 0.0;
+    driveTalonFXConfiguration.slot0.kD = 0.5;
     driveMotor.configAllSettings(driveTalonFXConfiguration);
 
     CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
@@ -89,8 +89,9 @@ public class SwerveModule extends SubsystemBase {
     }
 
 
-    // double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-    driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+    double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
+    // driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+    driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
   }
 
   /**