Actual unit tests

build.gradle

@@ -174,6 +174,11 @@ dependencies {
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher:1.10.2'
 
+
+    // Mockito for mocking objects in unit tests
+    testImplementation 'org.mockito:mockito-core:4.8.1'
+    testImplementation 'org.mockito:mockito-junit-jupiter:4.8.1'
+
     implementation 'gov.nist.math:jama:1.0.3'
 
     implementation "io.grpc:grpc-protobuf:${grpcVersion}"

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "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
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,10 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

src/main/java/frc/robot/BuildConstants.java

@@ -3,15 +3,15 @@
 /** Automatically generated file containing build version information. */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "4829-BaseRobotCode-1";
+  public static final String MAVEN_NAME = "4829-BaseRobotCode";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 32;
-  public static final String GIT_SHA = "71a47d311fd066ba3b76be8d09eff3e03b893cd8";
-  public static final String GIT_DATE = "2024-10-28 22:21:12 EDT";
-  public static final String GIT_BRANCH = "docs";
-  public static final String BUILD_DATE = "2024-10-29 16:56:45 EDT";
-  public static final long BUILD_UNIX_TIME = 1730235405242L;
-  public static final int DIRTY = 0;
+  public static final int GIT_REVISION = 15;
+  public static final String GIT_SHA = "f6cf5383d95d1116029cbb69e2db020295f5c2bf";
+  public static final String GIT_DATE = "2024-11-09 19:29:39 EST";
+  public static final String GIT_BRANCH = "actual-unit-tests";
+  public static final String BUILD_DATE = "2024-11-09 19:50:39 EST";
+  public static final long BUILD_UNIX_TIME = 1731199839115L;
+  public static final int DIRTY = 1;
 
   private BuildConstants() {}
 }

src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java

@@ -42,6 +42,8 @@ public class SwerveDrive extends SubsystemBase {
 
   private final Alert gyroDisconnectedAlert =
       new Alert("Gyro Hardware Fault", Alert.AlertType.kError);
+  private SwerveDriveKinematics kinematics;
+  private boolean isTest;
 
   public SwerveDrive(
       GyroInterface gyroIO,
@@ -53,6 +55,7 @@ public SwerveDrive(
     this.gyroInputs = new GyroInputsAutoLogged();
     this.rawGyroRotation = new Rotation2d();
 
+    setKinematics(DriveConstants.DRIVE_KINEMATICS);
     swerveModules =
         new SwerveModule[] {
           new SwerveModule(frontLeftModuleIO, "FrontLeft"),
@@ -68,9 +71,9 @@ public SwerveDrive(
           new SwerveModulePosition(),
           new SwerveModulePosition()
         };
-    this.poseEstimator =
+    poseEstimator =
         new SwerveDrivePoseEstimator(
-            DriveConstants.DRIVE_KINEMATICS,
+            getKinematics(),
             rawGyroRotation,
             lastModulePositions,
             new Pose2d(),
@@ -88,6 +91,14 @@ public SwerveDrive(
     gyroDisconnectedAlert.set(false);
   }
 
+  public SwerveDriveKinematics getKinematics() {
+    return kinematics;
+  }
+
+  public void setKinematics(SwerveDriveKinematics newKinematics) {
+    kinematics = newKinematics;
+  }
+
   /**
    * Gets the current velocity of the gyro's yaw
    *
@@ -160,7 +171,7 @@ public double getCharacterizationVelocity() {
   }
 
   /** Processes odometry inputs */
-  private void fetchOdometryInputs() {
+  void fetchOdometryInputs() {
     odometryThread.lockOdometry();
     odometryThread.updateInputs(odometryThreadInputs);
     Logger.processInputs("Drive/OdometryThread", odometryThreadInputs);
@@ -188,11 +199,12 @@ private void modulesPeriodic() {
    */
   public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative) {
     SwerveModuleState[] swerveModuleStates =
-        DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(
-            fieldRelative
-                ? ChassisSpeeds.fromFieldRelativeSpeeds(
-                    xSpeed, ySpeed, rotationSpeed, getPose().getRotation())
-                : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed));
+        getKinematics()
+            .toSwerveModuleStates(
+                fieldRelative
+                    ? ChassisSpeeds.fromFieldRelativeSpeeds(
+                        xSpeed, ySpeed, rotationSpeed, getPose().getRotation())
+                    : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed));
     SwerveDriveKinematics.desaturateWheelSpeeds(
         swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND);
 
@@ -216,7 +228,7 @@ public double getAllianceAngleOffset() {
    */
   public void setModuleStates(SwerveModuleState[] desiredStates) {
     for (int i = 0; i < 4; i++) {
-      swerveModules[i].runSetPoint(desiredStates[i]);
+      swerveModules[i].runSetpoint(desiredStates[i]);
     }
   }
 
@@ -232,7 +244,7 @@ private void addPoseEstimatorSwerveMeasurement(int timestampIndex) {
     if (gyroInputs.isConnected) {
       rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex];
     } else {
-      Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas);
+      Twist2d twist = getKinematics().toTwist2d(moduleDeltas);
       rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
     }
 

src/main/java/frc/robot/subsystems/swerve/SwerveModule.java

@@ -19,7 +19,7 @@ public class SwerveModule extends SubsystemBase {
   private final String name;
   private final ModuleInputsAutoLogged inputs = new ModuleInputsAutoLogged();
 
-  private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+  SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
 
   private final Alert hardwareFaultAlert;
 
@@ -57,7 +57,7 @@ public double getCharacterizationVelocity() {
   }
 
   /** Runs the module with the specified setpoint state. Returns the optimized state. */
-  public void runSetPoint(SwerveModuleState state) {
+  public void runSetpoint(SwerveModuleState state) {
     io.setDesiredState(state);
   }
 

src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java

@@ -21,5 +21,5 @@ public static class GyroInputs {
    *
    * @param inputs inputs to update
    */
-  void updateInputs(GyroInputs inputs);
+  default void updateInputs(GyroInputs inputs) {}
 }

src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java

@@ -31,20 +31,22 @@ class ModuleInputs {
    *
    * @param inputs the inputs to update
    */
-  void updateInputs(ModuleInputs inputs);
+  default void updateInputs(ModuleInputs inputs) {}
 
   /**
    * Sets the desired state for the module and sends calculated output from controller to the motor.
    *
    * @param desiredState Desired state with speed and angle.
    */
-  void setDesiredState(SwerveModuleState desiredState);
+  default void setDesiredState(SwerveModuleState desiredState) {}
 
-  void setDriveVoltage(Voltage voltage);
+  default void setDriveVoltage(Voltage voltage) {}
 
-  void setTurnVoltage(Voltage voltage);
+  default void setTurnVoltage(Voltage voltage) {}
 
-  void stopModule();
+  default void stopModule() {}
 
-  double getTurnRotations();
+  default double getTurnRotations() {
+    return 0.0;
+  }
 }

src/test/java/SwerveDriveTest.java

@@ -0,0 +1,83 @@
+import static org.junit.jupiter.api.Assertions.*;
+import static org.mockito.ArgumentMatchers.any;
+import static org.mockito.ArgumentMatchers.eq;
+import static org.mockito.Mockito.*;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import frc.robot.subsystems.swerve.*;
+import frc.robot.subsystems.swerve.gyroIO.GyroInterface;
+import frc.robot.subsystems.swerve.moduleIO.ModuleInterface;
+import frc.robot.subsystems.swerve.odometryThread.OdometryThread;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.mockito.Mock;
+import org.mockito.MockitoAnnotations;
+
+class SwerveDriveTest {
+
+  @Mock private GyroInterface gyroIO;
+
+  @Mock
+  private ModuleInterface frontLeftModuleIO,
+      frontRightModuleIO,
+      backLeftModuleIO,
+      backRightModuleIO;
+  @Mock private SwerveModule frontLeftModule, frontRightModule, backLeftModule, backRightModule;
+
+  @Mock private OdometryThread mockOdometryThread;
+
+  @Mock private SwerveDriveKinematics swerveDriveKinematics;
+
+  private SwerveDrive swerveDrive;
+
+  @BeforeEach
+  void setUp() {
+    // Initialize the mocks
+    MockitoAnnotations.openMocks(this);
+
+    swerveDrive =
+        spy(
+            new SwerveDrive(
+                gyroIO,
+                frontLeftModuleIO,
+                frontRightModuleIO,
+                backLeftModuleIO,
+                backRightModuleIO));
+
+    swerveDrive.setKinematics(swerveDriveKinematics);
+  }
+
+  @Test
+  void testDrive() {
+    // Prepare mock module states
+    SwerveModuleState[] mockStates = new SwerveModuleState[4];
+    for (int i = 0; i < 4; i++) {
+      mockStates[i] = mock(SwerveModuleState.class); // Mock each SwerveModuleState
+    }
+
+    // Mock the toSwerveModuleStates method to return mock states
+    when(swerveDriveKinematics.toSwerveModuleStates(any(ChassisSpeeds.class)))
+        .thenReturn(mockStates);
+
+    // Now when the drive method calls toSwerveModuleStates, it will return mockStates
+    swerveDrive.drive(1.0, 1.0, 0.5, true);
+
+    // Verify that setModuleStates was called with mockStates
+    verify(swerveDrive).setModuleStates(mockStates);
+    verify(swerveDrive.getKinematics()).toSwerveModuleStates(any(ChassisSpeeds.class));
+  }
+
+  @Test
+  void testSetPose() {
+    // Mock pose setter
+    Pose2d newPose = new Pose2d(3.0, 4.0, Rotation2d.fromDegrees(90));
+    swerveDrive.setPose(newPose);
+
+    // Verify that the setPose method was called with the new pose
+    verify(swerveDrive).setPose(eq(newPose));
+  }
+}