triangle path and some pid work

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":7.00293414379928,"y":1.995454266770603},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
+{"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}

src/main/deploy/pathplanner/Triangle.path

@@ -0,0 +1 @@
+{"waypoints":[{"anchorPoint":{"x":2.0,"y":3.0},"prevControl":null,"nextControl":{"x":3.0000000000000013,"y":3.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":3.0,"y":4.0},"nextControl":{"x":3.0,"y":4.0},"holonomicAngle":-90.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":3.0},"prevControl":{"x":3.0,"y":3.0},"nextControl":{"x":3.0,"y":3.0},"holonomicAngle":180.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":3.0},"prevControl":{"x":3.0271430576167138,"y":3.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]}

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

@@ -65,8 +65,8 @@ public final class Constants {
     public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 1.0, 0.0, 0, 1.0);
 
     // swerve auto constants
-    public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
-    public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
+    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 ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(
       1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI));
 

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

@@ -11,6 +11,8 @@ import java.util.Arrays;
 import java.util.HashMap;
 import java.util.List;
 
+import javax.swing.plaf.nimbus.State;
+
 import com.pathplanner.lib.PathPlanner;
 import com.pathplanner.lib.PathPlannerTrajectory;
 import com.pathplanner.lib.commands.PPSwerveControllerCommand;
@@ -123,6 +125,7 @@ public class RobotContainer {
     PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
     ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
     thetaController.enableContinuousInput(-Math.PI, Math.PI);
+    //thetaController.reset(new TrapezoidProfile.State(m_robotSwerveDrive.getOdometry()));
 
     PPSwerveControllerCommand ppSCC = new PPSwerveControllerCommand(
         traj,

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

@@ -48,12 +48,11 @@ public class SwerveModule extends SubsystemBase {
     angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
     angleMotor.configAllSettings(angleTalonFXConfiguration);
 
-    /*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
-    driveTalonFXConfiguration.slot0.kP = kDriveP;
-    driveTalonFXConfiguration.slot0.kI = kDriveI;
-    driveTalonFXConfiguration.slot0.kD = kDriveD;
-    driveTalonFXConfiguration.slot0.kF = kDriveF;
-    driveMotor.configAllSettings(driveTalonFXConfiguration);*/
+    TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
+    driveTalonFXConfiguration.slot0.kP = 1.0;
+    driveTalonFXConfiguration.slot0.kI = 0.0;
+    driveTalonFXConfiguration.slot0.kD = 0.0;
+    driveMotor.configAllSettings(driveTalonFXConfiguration);
 
     CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
     canCoderConfiguration.magnetOffsetDegrees = offset;
@@ -90,8 +89,8 @@ public class SwerveModule extends SubsystemBase {
     }
 
 
-    double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-    driveMotor.set(/*angleMotor.get() + */feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
+    // double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
+    driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
   }
 
   /**