Hopeful fixes from the first round of tests

lib/RoboLib

@@ -1 +1 @@
-Subproject commit 604e9e7cc50187f8233916384f4a5c1640f0fdc9
+Subproject commit bc53ef3b38fdbeee623ce0b188dbc7097b3cb405

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "030000004c050000cc09000011810000"
+    }
+  ]
+}

src/main/java/frc/robot/Robot.java

@@ -50,6 +50,7 @@ public class Robot extends LoggedRobot {
       // String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
       // Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
       // Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+      Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
     }
 
     // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in

src/main/java/frc/robot/RobotContainer.java

@@ -6,11 +6,13 @@ package frc.robot;
 
 import org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive.SwerveBuilder;
 import org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive.SwerveDriveSubsystem;
+import org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive.SwerveModule;
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.OperatorConstants;
@@ -32,6 +34,8 @@ public class RobotContainer {
   private final CommandXboxController m_driverController =
       new CommandXboxController(OperatorConstants.kDriverControllerPort);
 
+  // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
+
   public static SwerveDriveSubsystem driveSubsystem;
 
   private double driveP = 0.5;
@@ -52,6 +56,8 @@ public class RobotContainer {
     driveSubsystem = swerveBuilder
                     .withCanbusName("Drive")
                     .withPigeonId(20)
+                    .withMaxLinearVelocity(3.0)
+                    .withMaxAngularVelocity(Math.PI * 2)
                     .withSwerveModule(
                       swerveBuilder.new SwerveModuleBuilder()
                       .withDriveMotorId(12)
@@ -63,7 +69,7 @@ public class RobotContainer {
                       .withSteerKI(steerI)
                       .withSteerKD(steerD)
                       .withSteerEncoderId(10)
-                      .withModuleLocation(new Translation2d(-1, 1))
+                      .withModuleLocation(new Translation2d(1, 1))
                       .withModuleName("Front Left")
                       .withGearRatio(8.14)
                       .withWheelDiameter(0.1016)
@@ -80,11 +86,27 @@ public class RobotContainer {
                       .withSteerKI(steerI)
                       .withSteerKD(steerD)
                       .withSteerEncoderId(4)
-                      .withModuleLocation(new Translation2d(1, 1))
+                      .withModuleLocation(new Translation2d(1, -1))
                       .withModuleName("Front Right")
                       .withGearRatio(8.14)
                       .withWheelDiameter(0.1016)
                       .build())
+                    .withSwerveModule(
+                      swerveBuilder.new SwerveModuleBuilder()
+                      .withDriveMotorId(8)
+                      .withDriveKP(driveP)
+                      .withDriveKI(driveI)
+                      .withDriveKD(driveD)
+                      .withSteerMotorId(2)
+                      .withSteerKP(steerP)
+                      .withSteerKI(steerI)
+                      .withSteerKD(steerD)
+                      .withSteerEncoderId(7)
+                      .withModuleLocation(new Translation2d(-1, 1))
+                      .withModuleName("Back Left")
+                      .withGearRatio(8.14)
+                      .withWheelDiameter(0.1016)
+                      .build())
                     .withSwerveModule(
                       swerveBuilder.new SwerveModuleBuilder()
                       .withDriveMotorId(9)
@@ -95,25 +117,9 @@ public class RobotContainer {
                       .withSteerKP(steerP)
                       .withSteerKI(steerI)
                       .withSteerKD(steerD)
-                      .withSteerEncoderId(4)
+                      .withSteerEncoderId(4) // TODO: Duplicate ID
-                      .withModuleLocation(new Translation2d(1, -1))
-                      .withModuleName("Back Right")
-                      .withGearRatio(8.14)
-                      .withWheelDiameter(0.1016)
-                      .build())
-                      .withSwerveModule(
-                      swerveBuilder.new SwerveModuleBuilder()
-                      .withDriveMotorId(8)
-                      .withDriveKP(driveP)
-                      .withDriveKI(driveI)
-                      .withDriveKD(driveD)
-                      .withSteerMotorId(2)
-                      .withSteerKP(steerP)
-                      .withSteerKI(steerI)
-                      .withSteerKD(steerD)
-                      .withSteerEncoderId(7)
                       .withModuleLocation(new Translation2d(-1, -1))
-                      .withModuleName("Back Left")
+                      .withModuleName("Back Right")
                       .withGearRatio(8.14)
                       .withWheelDiameter(0.1016)
                       .build())
@@ -139,7 +145,7 @@ public class RobotContainer {
 
     // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
     // cancelling on release.
-    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
+    // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
   }
 
   public Command getDriveCommand(){
@@ -147,12 +153,21 @@ public class RobotContainer {
 
       @Override
       public void run() {
-        driveSubsystem.setChassisSpeedsFromJoystick(
+        driveSubsystem.setChassisSpeedsFromJoystickFieldRelative(
-          m_driverController.getLeftX() * Math.abs(m_driverController.getLeftX()),
+          -(m_driverController.getLeftY() * Math.abs(m_driverController.getLeftY())),
-          m_driverController.getLeftY() * Math.abs(m_driverController.getLeftY()),
+          -(m_driverController.getLeftX() * Math.abs(m_driverController.getLeftX())),
-          m_driverController.getRightX() * Math.abs(m_driverController.getRightX()));
+          -(m_driverController.getRawAxis(3) * Math.abs(m_driverController.getRawAxis(3))));
 
         Logger.recordOutput("SwerveState", driveSubsystem.getModuleStates());
+        Logger.recordOutput("Pose Estimation", driveSubsystem.getRobotPose());
+
+        for(int i = 0;i<driveSubsystem.getNumberOfModules();i++){
+          SwerveModule mod = driveSubsystem.getModules()[i];
+          String modName = mod.getModuleName();
+
+          Logger.recordOutput(modName + " wheel speed", mod.getDriveWheelSpeed());
+          Logger.recordOutput(modName + " steer rotation", mod.getSteerRotations());
+        }
       }
 
     }, driveSubsystem);