Merge remote-tracking branch 'origin/swerve' into robot-logger

build.gradle

@@ -28,7 +28,7 @@ deploy {
 
                 frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
                 // Enable visualvm
-                // jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2"                     
+                jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2"                     
                 }
 
                 // Static files artifact

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

@@ -0,0 +1 @@
+{"waypoints":[{"anchorPoint":{"x":8.334720117716095,"y":1.881355486848905},"prevControl":null,"nextControl":{"x":8.61849999562068,"y":1.250733535949832},"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.520125502663889,"y":1.354445775900491},"nextControl":{"x":4.750685363439692,"y":2.5855499986862043},"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/L Path copy.path

@@ -0,0 +1,37 @@
+{
+    "waypoints": [
+        {
+            "anchorPoint": {
+                "x": 5.0,
+                "y": 2.0
+            },
+            "prevControl": null,
+            "nextControl": {
+                "x": 5.004218182347978,
+                "y": 2.0
+            },
+            "holonomicAngle": 0.0,
+            "isReversal": false,
+            "velOverride": null,
+            "isLocked": false
+        },
+        {
+            "anchorPoint": {
+                "x": 5.0,
+                "y": 5.0
+            },
+            "prevControl": {
+                "x": 4.991875448297241,
+                "y": 5.0
+            },
+            "nextControl": null,
+            "holonomicAngle": 0.0,
+            "isReversal": false,
+            "velOverride": null,
+            "isLocked": false
+        }
+    ],
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "isReversed": null
+}

src/main/deploy/pathplanner/L Path.path

@@ -0,0 +1,55 @@
+{
+    "waypoints": [
+        {
+            "anchorPoint": {
+                "x": 2.0,
+                "y": 2.0
+            },
+            "prevControl": null,
+            "nextControl": {
+                "x": 3.0,
+                "y": 2.0
+            },
+            "holonomicAngle": 0.0,
+            "isReversal": false,
+            "velOverride": null,
+            "isLocked": false
+        },
+        {
+            "anchorPoint": {
+                "x": 5.0,
+                "y": 2.0
+            },
+            "prevControl": {
+                "x": 4.000000000000002,
+                "y": 2.0
+            },
+            "nextControl": {
+                "x": 5.004218182347978,
+                "y": 2.0
+            },
+            "holonomicAngle": 0.0,
+            "isReversal": false,
+            "velOverride": null,
+            "isLocked": false
+        },
+        {
+            "anchorPoint": {
+                "x": 5.0,
+                "y": 5.0
+            },
+            "prevControl": {
+                "x": 4.991875448297241,
+                "y": 5.0
+            },
+            "nextControl": null,
+            "holonomicAngle": 0.0,
+            "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.055485973040912,"y":4.3828225587485665},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":5.0,"y":3.1241654489412927},"nextControl":{"x":5.0,"y":3.1241654489412927},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":{"x":5.0,"y":3.0836227700747108},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}

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

@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":2.0390109745736735,"y":2.9639231692256494},"prevControl":null,"nextControl":{"x":2.049521340421991,"y":2.9534128033773315},"holonomicAngle":1.2188752351312955,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":5.0029341437993216,"y":2.995454266770603},"nextControl":null,"holonomicAngle":-178.60254149056965,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.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":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":{"x":1.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}

src/main/deploy/pathplanner/Test.path

@@ -0,0 +1 @@
+{"waypoints":[{"anchorPoint":{"x":3.1215786569504176,"y":1.6921689015791839},"prevControl":null,"nextControl":{"x":1.7026792674275004,"y":1.7552310966690905},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.20566158373696,"y":3.3948481690066825},"prevControl":{"x":1.9969695111804016,"y":3.342296339765093},"nextControl":{"x":4.204717742092026,"y":3.4382853932829898},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.076506704737548,"y":2.701164023017701},"prevControl":{"x":4.771706095136329,"y":3.037495730163874},"nextControl":{"x":5.592408962233515,"y":2.131892566470425},"holonomicAngle":92.0952525645957,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.884289630648579,"y":1.7762518283676958},"prevControl":{"x":5.57049389960884,"y":1.7342103649744238},"nextControl":{"x":8.154698525195206,"y":1.816904912993188},"holonomicAngle":-177.33699923393286,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.8002067038620355,"y":3.4473999982502432},"prevControl":{"x":8.681562190710942,"y":3.5525036567334225},"nextControl":{"x":4.918851217013129,"y":3.3422963397670644},"holonomicAngle":-177.2241965512169,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.111068291102451,"y":1.681658535732834},"prevControl":{"x":5.139568899827807,"y":1.8393140234576038},"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

@@ -4,9 +4,12 @@
 
 package frc4388.robot;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
 
@@ -27,8 +30,8 @@ public final class Constants {
   public static final class SwerveDriveConstants {
     public static final double ROTATION_SPEED = 4;
     public static final double WHEEL_SPEED = 0.1;
-    public static final double WIDTH = 15.27;
+    public static final double WIDTH = 15.25;
-    public static final double HEIGHT = 15.27;
+    public static final double HEIGHT = 15.25;
     public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
     public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
     public static final double MAX_SPEED_FEET_PER_SEC = 20; // redundant constant?
@@ -61,6 +64,12 @@ public final class Constants {
     public static final int SWERVE_TIMEOUT_MS = 30;
     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.5, 0.0, 0.0);
+    public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
+    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(
+            15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
+
     // swerve configuration
     public static final double NEUTRAL_DEADBAND = 0.04;
     public static final double OPEN_LOOP_RAMP_RATE = 0.2;
@@ -71,7 +80,7 @@ public final class Constants {
     // wheel diameter: official = 4 in, measured = 3.8 in
     /* Ratio Calculation */
     public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
-    public static final double WHEEL_DIAMETER_INCHES = 3.8;
+    public static final double WHEEL_DIAMETER_INCHES = 4.0;
     public static final double TICKS_PER_MOTOR_REV = 2048;
     public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
     public static final double INCHES_PER_METER = 39.370;

src/main/java/frc4388/robot/Robot.java

@@ -127,17 +127,6 @@ public class Robot extends TimedRobot {
       e.printStackTrace();
     }
 
-    /*String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
-    switch (autoSelected) {
-      case "My Auto":
-        autonomousCommand = new MyAutoCommand();
-        break;
-      case "Default Auto":
-      default:
-        autonomousCommand = new ExampleCommand();
-        break;
-    }*/
-
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();

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

@@ -4,33 +4,34 @@
 
 package frc4388.robot;
 
-import java.io.File;
 import java.io.IOException;
-import java.util.Arrays;
+import java.util.ArrayList;
-import java.util.Comparator;
 
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.pathplanner.lib.PathPlanner;
 import com.pathplanner.lib.PathPlannerTrajectory;
+import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
 import com.pathplanner.lib.commands.PPSwerveControllerCommand;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
-import frc4388.utility.PathPlannerTrajectoryUtil;
 import frc4388.utility.controller.DeadbandedXboxController;
 
 /**
@@ -48,6 +49,8 @@ public class RobotContainer {
   private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
       m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
 
+  private final TalonFX m_testMotor = new TalonFX(23);
+
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
   /* Controllers */
@@ -72,6 +75,8 @@ public class RobotContainer {
 
     // continually sends updates to the Blinkin LED controller to keep the lights on
     m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
+
+    m_testMotor.set(TalonFXControlMode.PercentOutput, 0);
   }
 
   /**
@@ -97,9 +102,7 @@ public class RobotContainer {
         .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
 
     new JoystickButton(getDriverController(), XboxController.Button.kA.value)
-
+      .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
-        .whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
-    // .whenPressed(this::resetOdometry);
 
     /* Operator Buttons */
     // activates "Lit Mode"
@@ -109,6 +112,46 @@ public class RobotContainer {
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
   }
 
+  public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) {
+
+    ArrayList<Command> commands = new ArrayList<Command>();
+    commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
+
+    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
+    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
+    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
+    thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+    // parse input
+    for (int i=0; i<inputs.length; i++) {
+
+      if (inputs[i] instanceof String) {
+        PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
+        PathPlannerState initState = (PathPlannerState) traj.sample(0);
+
+        commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
+        commands.add(new PPSwerveControllerCommand(
+                          traj,
+                          m_robotSwerveDrive::getOdometry,
+                          m_robotSwerveDrive.m_kinematics,
+                          xController,
+                          yController,
+                          thetaController,
+                          m_robotSwerveDrive::setModuleStates,
+                          m_robotSwerveDrive));
+      }
+
+      if (inputs[i] instanceof Command) {
+        commands.add((Command) inputs[i]);
+      }
+    }
+    
+    commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
+    Command[] ret = new Command[commands.size()];
+    ret = commands.toArray(ret);
+    return ret;
+  }
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
@@ -118,34 +161,9 @@ public class RobotContainer {
   public Command getAutonomousCommand() throws IOException {
     // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
 
-    try (
+    Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down");
-        PIDController xController = new PIDController(10.0, 0.0, 0.0);
+    SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands);
-        PIDController yController = new PIDController(1.3, 0.0, 0.0)) {
+    return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2)))));
-      ProfiledPIDController thetaController = new ProfiledPIDController(
-          10.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI));
-      thetaController.enableContinuousInput(-Math.PI, Math.PI);
-      PathPlannerTrajectory ppRecorded = PathPlannerTrajectoryUtil
-          .fromPathweaverJson(Arrays.stream(Filesystem.getDeployDirectory().listFiles())
-              .max(Comparator.comparingLong(File::lastModified)).orElseThrow().toPath(), 1.0, 1.0);
-
-      PathPlannerTrajectory ppCurrentPath = ppRecorded; // change this to change auto
-
-      PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand(
-          ppCurrentPath,
-          m_robotSwerveDrive::getOdometry,
-          m_robotSwerveDrive.m_kinematics,
-          xController,
-          yController,
-          thetaController,
-          m_robotSwerveDrive::setModuleStates,
-          m_robotSwerveDrive);
-
-      return new SequentialCommandGroup(
-          new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
-          new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrentPath.getInitialPose())),
-          ppSwerveControllerCommand,
-          new InstantCommand(m_robotSwerveDrive::stopModules));
-    }
   }
 
   /**
@@ -159,7 +177,7 @@ public class RobotContainer {
     return m_robotSwerveDrive.getOdometry();
   }
 
-  public void zeroOdometry(Pose2d pose) {
+  public void resetOdometry(Pose2d pose) {
     m_robotSwerveDrive.resetOdometry(pose);
   }
 

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

@@ -14,6 +14,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -50,9 +51,12 @@ public class SwerveDrive extends SubsystemBase {
   /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
   below are robot specific, and should be tuned. */
   public SwerveDrivePoseEstimator m_poseEstimator;
+  public SwerveDriveOdometry m_odometry;
 
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
+  public Rotation2d rotTarget = new Rotation2d();;
+  private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   private final Field2d m_field = new Field2d();
 
@@ -89,9 +93,12 @@ public class SwerveDrive extends SubsystemBase {
           m_gyro.getRotation2d(),
           new Pose2d(),
           m_kinematics,
-          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+          VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
-          VecBuilder.fill(Units.degreesToRadians(0.01)),
+          VecBuilder.fill(Units.degreesToRadians(1)),
-          VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+          VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
+
+    m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
+
     m_gyro.reset(); 
     SmartDashboard.putData("Field", m_field);
   }
@@ -121,8 +128,7 @@ public class SwerveDrive extends SubsystemBase {
               : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
     setModuleStates(states);
   }
-  private Rotation2d rotTarget = new Rotation2d();
+
-  private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
   public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
   {
     ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
@@ -153,7 +159,7 @@ public class SwerveDrive extends SubsystemBase {
   
   @Override
   public void periodic() {
-    //System.err.println(m_gyro.getFusedHeading() +"    aaa");
+    // System.err.println(m_gyro.getFusedHeading() +"    aaa");
     updateOdometry();
     // m_gyro.setFusedHeadingToCompass();
     // m_gyro.setYawToCompass();
@@ -182,7 +188,7 @@ public class SwerveDrive extends SubsystemBase {
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
 
-    m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
+    // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
     super.periodic();
   }
 
@@ -217,6 +223,7 @@ public class SwerveDrive extends SubsystemBase {
    * @return Robot's current pose.
    */
   public Pose2d getOdometry() {
+    // return m_odometry.getPoseMeters();
     return m_poseEstimator.getEstimatedPosition();
   }
 
@@ -224,16 +231,24 @@ public class SwerveDrive extends SubsystemBase {
    * Resets the odometry of the robot to (x=0, y=0, theta=0).
    */
   public void resetOdometry(Pose2d pose) {
+    // m_odometry.resetPosition(pose, m_gyro.getRotation2d());
     m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
   }
 
   /** Updates the field relative position of the robot. */
+
   public void updateOdometry() {
     m_poseEstimator.update( m_gyro.getRotation2d(), 
                             modules[0].getState(), 
                             modules[1].getState(), 
                             modules[2].getState(), 
                             modules[3].getState());
+    
+    // m_odometry.update( m_gyro.getRotation2d(), 
+    //                    modules[0].getState(),
+    //                    modules[1].getState(),
+    //                    modules[2].getState(),
+    //                    modules[3].getState());
   
       // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
       // a real robot, this must be calculated based either on latency or timestamps.
@@ -242,6 +257,11 @@ public class SwerveDrive extends SubsystemBase {
       //     Timer.getFPGATimestamp() - 0.1);
     }
 
+  public void resetGyro(){
+    m_gyro.reset();
+    rotTarget = new Rotation2d(0);
+  }
+
   public void stopModules() {
     modules[0].stop();
     modules[1].stop();

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();
+    TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
-    driveTalonFXConfiguration.slot0.kP = kDriveP;
+    driveTalonFXConfiguration.slot0.kP = 0.15;
-    driveTalonFXConfiguration.slot0.kI = kDriveI;
+    driveTalonFXConfiguration.slot0.kI = 0.0;
-    driveTalonFXConfiguration.slot0.kD = kDriveD;
+    driveTalonFXConfiguration.slot0.kD = 0.5;
-    driveTalonFXConfiguration.slot0.kF = kDriveF;
+    driveMotor.configAllSettings(driveTalonFXConfiguration);
-    driveMotor.configAllSettings(driveTalonFXConfiguration);*/
 
     CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
     canCoderConfiguration.magnetOffsetDegrees = offset;
@@ -91,7 +90,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);
+    // driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+    driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
   }
 
   /**