From f974e6545a5ad9bcaff52231006730878ea58fd1 Mon Sep 17 00:00:00 2001
From: Kaya <95276965+kytpbs@users.noreply.github.com>
Date: Thu, 30 May 2024 18:19:29 +0300
Subject: [PATCH] Create DriveWithGyroTests.java

---
 .../DriveWithGyroTests.java                   | 62 +++++++++++++++++++
 1 file changed, 62 insertions(+)
 create mode 100644 src/test/java/subsystem_tests/drive_subsystem_tests/DriveWithGyroTests.java

diff --git a/src/test/java/subsystem_tests/drive_subsystem_tests/DriveWithGyroTests.java b/src/test/java/subsystem_tests/drive_subsystem_tests/DriveWithGyroTests.java
new file mode 100644
index 0000000..4e17764
--- /dev/null
+++ b/src/test/java/subsystem_tests/drive_subsystem_tests/DriveWithGyroTests.java
@@ -0,0 +1,62 @@
+package subsystem_tests.drive_subsystem_tests;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants.DriveConstants;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import subsystem_tests.drive_subsystem_tests.utils.NavXSim;
+
+class DriveWithGyroTests extends DriveSubsystemTestBase {
+
+  @BeforeEach
+  public void setUp() {
+    super.setUp();
+  }
+
+  @AfterEach
+  public void tearDown() {
+    super.tearDown();
+  }
+
+  @Test
+  void printAllSimNames() {
+    for (int i = 1; i <= 16; i++) {
+      var deviceName = SimDeviceDataJNI.getSimDeviceName(i);
+      System.out.println(deviceName);
+    }
+  }
+
+  @Test
+  void testDriveForwardWithAngleFieldRelative() {
+    Timer.delay(0.02); // let the navX thread update
+    NavXSim.setConnected(true);
+    NavXSim.setAngle(90);
+    Timer.delay(0.1); // wait 20ms for the navX Thread to update
+    driveSubsystem.drive(0.5, 0, 0, true, false);
+
+    for (var state : driveSubsystem.getModuleDesiredStates()) {
+      assertEquals(DriveConstants.kMaxSpeedMetersPerSecond * 0.5, state.speedMetersPerSecond, 0.01);
+      assertEquals(Rotation2d.fromDegrees(90), state.angle);
+    }
+  }
+
+  @Test
+  void testDriveSidewaysWithAngleFieldRelative() {
+    Timer.delay(0.02); // wait 20ms for the navX Thread to update
+    NavXSim.setConnected(true);
+    NavXSim.setAngle(45);
+    Timer.delay(0.1); // wait 20ms for the navX Thread to update
+    driveSubsystem.drive(0, 0.5, 0, true, false);
+
+    for (var state : driveSubsystem.getModuleDesiredStates()) {
+      assertEquals(DriveConstants.kMaxSpeedMetersPerSecond * 0.5, state.speedMetersPerSecond, 0.01);
+      // 90 + 45 degrees
+      assertEquals(Rotation2d.fromDegrees(90 + 45), state.angle);
+    }
+  }
+}