From 8fa4907c1e412e3771b2d4a35f114266281ea075 Mon Sep 17 00:00:00 2001
From: bovlb <31326650+bovlb@users.noreply.github.com>
Date: Sun, 5 Jan 2025 22:01:00 -0800
Subject: [PATCH] Try writing elevator tests

---
 .../robot/subsystems/ElevatorSubsystem.java   | 14 +++++-
 src/test/java/TestElevatorSubsystem.java      | 50 +++++++++++++++++++
 2 files changed, 63 insertions(+), 1 deletion(-)
 create mode 100644 src/test/java/TestElevatorSubsystem.java

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index 0dcc57b..ab8c11c 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -6,11 +6,13 @@
 
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.sim.SparkFlexSim;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class ElevatorSubsystem extends SubsystemBase {
+public class ElevatorSubsystem extends SubsystemBase implements AutoCloseable {
   // motor (neo vortex according to co-engineering pres?)
   private SparkFlex m_elevatorMotor = new SparkFlex(0, MotorType.kBrushless);
   // pid
@@ -40,4 +42,14 @@ public void setPosition(double position) {
   public void periodic() {
     // This method will be called once per scheduler run
   }
+
+  public SparkFlexSim getSimMotor() {
+    return new SparkFlexSim(m_elevatorMotor,
+      DCMotor.getNeoVortex(1));
+  }
+
+  @Override
+  public void close() {
+    m_elevatorMotor.close();
+  }
 }
diff --git a/src/test/java/TestElevatorSubsystem.java b/src/test/java/TestElevatorSubsystem.java
new file mode 100644
index 0000000..11fdd2f
--- /dev/null
+++ b/src/test/java/TestElevatorSubsystem.java
@@ -0,0 +1,50 @@
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import edu.wpi.first.hal.HAL;
+import frc.robot.subsystems.ElevatorSubsystem;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.sim.SparkFlexSim;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+public class TestElevatorSubsystem {
+    private ElevatorSubsystem m_elevatorSubsystem;
+    private SparkFlexSim m_simElevatorMotor;
+
+    @BeforeEach
+    public void setup() {
+        assert(HAL.initialize(500, 0));
+        m_elevatorSubsystem = new ElevatorSubsystem();
+        m_simElevatorMotor = m_elevatorSubsystem.getSimMotor();  
+    }
+
+    @Test
+    public void testPower() {        
+        assertEquals(m_simElevatorMotor.getAppliedOutput(), 0.0);
+        m_elevatorSubsystem.setPower(0.5);
+        assertEquals(m_simElevatorMotor.getAppliedOutput(), 0.5);
+        m_elevatorSubsystem.setPower(1.0);
+        assertEquals(m_simElevatorMotor.getAppliedOutput(), 1.0);
+    }
+
+    @Test void testPosition() {
+        assertEquals(m_simElevatorMotor.getSetpoint(), 0);
+        m_elevatorSubsystem.setPosition(0.5);
+        assertEquals(m_simElevatorMotor.getSetpoint(), 0.5);
+        m_elevatorSubsystem.setPosition(1.0);
+        assertEquals(m_simElevatorMotor.getSetpoint(), 1.0);
+    }
+
+    @Test void testPosition2() {
+        m_elevatorSubsystem.setPosition(1.0);
+        for (int i = 0; i < 100; i++) {
+            m_elevatorSubsystem.simulationPeriodic();
+        }
+        assertEquals(m_simElevatorMotor.getPosition(), 1.0, 0.02);
+    }
+
+    @AfterEach
+    public void tearDown() {
+        m_elevatorSubsystem.close();
+    }
+}