Cleaned up some dead code and wrote some unit tests

src/main/java/cshcyberhawks/lib/requests/AwaitRequest.kt

@@ -1,5 +1,10 @@
 package cshcyberhawks.lib.requests
 
+/**
+ * A request that waits until a specified condition is met.
+ *
+ * @property condition A lambda function that returns a Boolean. The request will wait until this condition evaluates to true.
+ */
 class AwaitRequest(private val condition: () -> Boolean) : Request() {
     override fun execute() {}
 

src/main/java/frc/robot/math/MiscCalculations.kt

@@ -43,13 +43,6 @@ object MiscCalculations {
         return abs(n1 - n2) <= range
     }
 
-    /**
-     * A function to get the current time in milliseconds
-     *
-     * @return The current time in milliseconds
-     */
-    fun getCurrentTime(): Double = WPIUtilJNI.now() * 1.0e-6
-
     fun translation2dWithinRange(current: Translation2d, range: Pair<Translation2d, Translation2d>): Boolean {
         val rangeStart = range.first
         val rangeEnd = range.second
@@ -92,11 +85,6 @@ object MiscCalculations {
 //    }
 
 
-    // rpm to units per sec conversion factor
-    // (rpm) * 2pi (rad/r) * r (r units/rad) * 1/60 (min/s)
-    fun rpm2ups(r: Double): Double = PI * r / 30.0
-    fun ups2rpm(r: Double): Double = 30.0 / (PI * r)
-
     /**
      * Converts a speed/acceleration in units to motor rotations.
      *

src/main/java/frc/robot/subsystems/superstructure/SuperstructureAction.kt

@@ -1,81 +1,8 @@
 package frc.robot.subsystems.superstructure
 
 import cshcyberhawks.lib.requests.*
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
-import edu.wpi.first.wpilibj2.command.Command
-import edu.wpi.first.wpilibj2.command.Commands
 import frc.robot.RobotState
 
-//class SuperstructureAction(
-//    private val prepAction: Request,
-//    private val confirmAction: Request,
-//    private val returnAction: Request,
-//    private val confirmed: () -> Boolean = { RobotState.actionConfirmed },
-//    private val cancelled: () -> Boolean = { RobotState.actionCancelled }
-//) : Request() {
-//    private var confirmActionScheduled = false
-//    private var returnActionScheduled = false
-//    private var cancelledCaught = false
-//
-//    private var currentCommand = EmptyRequest()
-//
-//    private fun setCurrentAction(cmd: Request) {
-//        currentCommand = cmd
-//        currentCommand.execute()
-//    }
-//
-//    override fun initialize() {
-//        RobotState.actionCancelled = false
-//        RobotState.actionConfirmed = false
-//
-//        confirmActionScheduled = false
-//        returnActionScheduled = false
-//        cancelledCaught = false
-//
-//        setCurrentAction(prepAction)
-//    }
-//
-//    override fun execute() {
-//        SmartDashboard.putBoolean("Prep Finished", prepAction.isFinished)
-//        SmartDashboard.putBoolean("Confirm Finished", confirmAction.isFinished)
-//        SmartDashboard.putBoolean("Return Finished", returnAction.isFinished)
-//        SmartDashboard.putBoolean("Current Finished", currentCommand.isFinished)
-//
-//        if (cancelledCaught) return
-//
-////        println(cancelled())
-//
-//        if (cancelled()) {
-//            println("Cancelled")
-//            cancelledCaught = true
-//            RobotState.actionCancelled = false
-//            setCurrentAction(returnAction)
-//            returnActionScheduled = true
-//        }
-//
-//        if (prepAction.isFinished && confirmed()) {
-//            println("Confirmed")
-//            RobotState.actionConfirmed = false
-//            setCurrentAction(confirmAction)
-//            confirmActionScheduled = true
-//        }
-//
-//        if (confirmActionScheduled && confirmAction.isFinished) {
-//            setCurrentAction(returnAction)
-//            returnActionScheduled = true
-//        }
-//    }
-//
-//    override fun isFinished(): Boolean {
-//        return returnActionScheduled && returnAction.isFinished
-//    }
-//
-//    override fun end(interrupted: Boolean) {
-//        RobotState.actionCancelled = false
-//        RobotState.actionConfirmed = false
-//    }
-//}
-
 object SuperstructureAction {
     fun create(
         prepAction: Request,

src/test/java/frc/robot/math/MiscCalculationsTest.kt

@@ -1,12 +1,61 @@
 package frc.robot.math
 
+import edu.wpi.first.math.geometry.Translation2d
 import org.junit.jupiter.api.Test
 
 import org.junit.jupiter.api.Assertions.*
 
 internal class MiscCalculationsTest {
-    // To learn more about how to write unit tests, see the
-    // JUnit 5 User Guide at https://junit.org/junit5/docs/current/user-guide/
+    @Test
+    fun testGToMetersPerSecond() {
+        assertEquals(9.8066, MiscCalculations.gToMetersPerSecond(1.0), 1e-4)
+        assertEquals(19.6132, MiscCalculations.gToMetersPerSecond(2.0), 1e-4)
+        assertEquals(0.0, MiscCalculations.gToMetersPerSecond(0.0), 1e-4)
+    }
+
+    @Test
+    fun testCalculateDeadzone() {
+        assertEquals(0.0, MiscCalculations.calculateDeadzone(0.1, 0.2), 1e-4)
+        assertEquals(0.0, MiscCalculations.calculateDeadzone(-0.1, 0.2), 1e-4)
+        assertEquals(0.5, MiscCalculations.calculateDeadzone(0.6, 0.2), 1e-4)
+        assertEquals(-0.5, MiscCalculations.calculateDeadzone(-0.6, 0.2), 1e-4)
+    }
+
+    @Test
+    fun testAppxEqual() {
+        assertTrue(MiscCalculations.appxEqual(1.0, 1.05, 0.1))
+        assertFalse(MiscCalculations.appxEqual(1.0, 1.2, 0.1))
+        assertTrue(MiscCalculations.appxEqual(-1.0, -1.05, 0.1))
+    }
+
+    @Test
+    fun testTranslation2dWithinRange() {
+        val range = Pair(Translation2d(0.0, 0.0), Translation2d(2.0, 2.0))
+        assertTrue(MiscCalculations.translation2dWithinRange(Translation2d(1.0, 1.0), range))
+        assertFalse(MiscCalculations.translation2dWithinRange(Translation2d(3.0, 1.0), range))
+        assertFalse(MiscCalculations.translation2dWithinRange(Translation2d(1.0, 3.0), range))
+    }
+
+    @Test
+    fun testFindMatchingTranslation2dRange() {
+        val ranges = arrayOf(
+            Pair(Translation2d(0.0, 0.0), Translation2d(2.0, 2.0)),
+            Pair(Translation2d(3.0, 3.0), Translation2d(5.0, 5.0))
+        )
+        val defaultRange = Pair(Translation2d(-1.0, -1.0), Translation2d(-1.0, -1.0))
+        assertEquals(
+            ranges[0],
+            MiscCalculations.findMatchingTranslation2dRange(Translation2d(1.0, 1.0), ranges, defaultRange)
+        )
+        assertEquals(
+            ranges[1],
+            MiscCalculations.findMatchingTranslation2dRange(Translation2d(4.0, 4.0), ranges, defaultRange)
+        )
+        assertEquals(
+            defaultRange,
+            MiscCalculations.findMatchingTranslation2dRange(Translation2d(6.0, 6.0), ranges, defaultRange)
+        )
+    }
 
     @Test
     fun testPositionToRotations() {