unit tests for speakershot

awtybots · Apr 5, 2024 · bc6dc04 · bc6dc04
1 parent b5175aa
commit bc6dc04
Show file tree

Hide file tree

Showing 11 changed files with 130 additions and 25 deletions.
diff --git a/build.gradle b/build.gradle
@@ -91,8 +91,9 @@ dependencies {
     nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
     simulationRelease wpi.sim.enableRelease()
 
-    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+    testImplementation(platform('org.junit:junit-bom:5.10.2'))
+    testImplementation('org.junit.jupiter:junit-jupiter')
+    testRuntimeOnly('org.junit.platform:junit-platform-launcher')
 
     implementation 'gov.nist.math:jama:1.0.3'
 
@@ -103,6 +104,9 @@ dependencies {
 test {
     useJUnitPlatform()
     systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+    testLogging {
+        events "passed", "skipped", "failed"
+    }
 }
 
 // Simulation configuration (e.g. environment variables).

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -142,13 +142,17 @@ public static final class ArmConstants {
     public static final double minimumAngle = 0;
     public static final double maximumAngle = Math.PI;
 
-    public static final double uprightAngle = 1.734; // (for gravity calculations for PID)
-
-    // do not use
-    // public static final double kFF = 0.0;
-    // public static final double kIz = 0.0;
-
-    // public static final double initialAngle = 0.0; // Not sure what this is for
+    // Quadratic regression calculated a, b, and c values. For SpeakerShot and tests.
+    // TODO CALCULATE REAL VALUES (this is a temporary rough calculation)
+    public static final double QUADRATIC_A = -0.169338;
+    public static final double QUADRATIC_B = 0.803248;
+    public static final double QUADRATIC_C = 0.0;
+
+    public static final double UpwardsAngle = 1.753; // (for gravity calculations for PID and tests)
+    public static final double FloorPickupAngle = 0.09; // ditto
+    public static final double ShootCloseAngle = 0.747;
+    public static final double ShootMediumAngle = 0.933;
+    public static final double ShootFarAngle = 0.799;
   }
 
   public static final class Presets {

diff --git a/src/main/java/frc/robot/commands/Positions/FloorPickup.java b/src/main/java/frc/robot/commands/Positions/FloorPickup.java
@@ -14,6 +14,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.arm.Arm;
 import java.util.Optional;
 
@@ -23,7 +24,7 @@ public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ArmAngle = 0.09;
+              double ArmAngle = ArmConstants.FloorPickupAngle;
 
               arm.runTargetAngle(Optional.of(ArmAngle));
             },

diff --git a/src/main/java/frc/robot/commands/Positions/ShootClose.java b/src/main/java/frc/robot/commands/Positions/ShootClose.java
@@ -14,6 +14,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.arm.Arm;
 import java.util.Optional;
 
@@ -23,7 +24,7 @@ public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ArmAngle = 0.747;
+              double ArmAngle = ArmConstants.ShootCloseAngle;
 
               arm.runTargetAngle(Optional.of(ArmAngle));
             },

diff --git a/src/main/java/frc/robot/commands/Positions/ShootFar.java b/src/main/java/frc/robot/commands/Positions/ShootFar.java
@@ -14,6 +14,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.arm.Arm;
 import java.util.Optional;
 
@@ -23,7 +24,7 @@ public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ArmAngle = 0.799;
+              double ArmAngle = ArmConstants.ShootFarAngle;
               arm.runTargetAngle(Optional.of(ArmAngle));
             },
             arm)

diff --git a/src/main/java/frc/robot/commands/Positions/ShootMedium.java b/src/main/java/frc/robot/commands/Positions/ShootMedium.java
@@ -14,6 +14,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.arm.Arm;
 import java.util.Optional;
 
@@ -23,7 +24,7 @@ public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ArmAngle = 0.933;
+              double ArmAngle = ArmConstants.ShootMediumAngle;
 
               arm.runTargetAngle(Optional.of(ArmAngle));
             },

diff --git a/src/main/java/frc/robot/commands/Positions/SpeakerShot.java b/src/main/java/frc/robot/commands/Positions/SpeakerShot.java
@@ -15,6 +15,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.arm.Arm;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.util.AllianceFlipUtil;
@@ -61,21 +62,22 @@ public static Command run(Arm arm, Drive drive) {
   public static Optional<Double> getRequiredAngle(Optional<Double> speakerDistance) {
     // Numbers calculated using Desmos. Note that the range is between 1.753 (upwards) and 0.09
     // (floor pickup) at the moment. Polynomial form ax^2 + bx + c.
-    final double a = 1.0;
-    final double b = 1.0;
-    final double c = 1.0;
+
+    if (-ArmConstants.QUADRATIC_B * Math.pow((2 * ArmConstants.QUADRATIC_A), -1) < 0) {}
 
     final double calculatedArmAngle =
-        a * Math.pow(speakerDistance.get(), 2) + b * speakerDistance.get() + c;
+        ArmConstants.QUADRATIC_A * Math.pow(speakerDistance.get(), 2)
+            + ArmConstants.QUADRATIC_B * speakerDistance.get()
+            + ArmConstants.QUADRATIC_C;
 
-    // TODO once this is merged into main, make the FloorPickup and the Upwards angles be constants
     // and reference them for here so they can update nicely.
 
-    if (Math.abs(calculatedArmAngle) < 1.753 && Math.abs(calculatedArmAngle) > 0.09) {
-      System.err.println(
-          "ERROR: The calculated SpeakerShot angle is outside of the allowed range for the arm.");
-      return Optional.empty();
-    }
+    // if (Math.abs(calculatedArmAngle) < 1.753 && Math.abs(calculatedArmAngle) > 0.09) {
+    //   System.err.println(
+    //       "ERROR: The calculated SpeakerShot angle is outside of the allowed range for the
+    // arm.");
+    //   return Optional.empty();
+    // }
 
     if (speakerDistance.isEmpty()) {
       System.err.println("WARNING: No speakerDistance detected for SpeakerShot.");

diff --git a/src/main/java/frc/robot/commands/Positions/Upwards.java b/src/main/java/frc/robot/commands/Positions/Upwards.java
@@ -14,6 +14,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.arm.Arm;
 import java.util.Optional;
 
@@ -23,7 +24,7 @@ public static Command run(Arm arm) {
     return Commands.run(
         () -> {
           // Position preset settings
-          double ArmAngle = 1.753;
+          double ArmAngle = ArmConstants.UpwardsAngle;
 
           arm.runTargetAngle(Optional.of(ArmAngle));
         },

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java
@@ -111,7 +111,7 @@ private double getSmoothedPosition() {
   }
 
   public double getAngleFromVertical() {
-    return (getSmoothedPosition() * Math.PI * 2.0 - Constants.ArmConstants.uprightAngle);
+    return (getSmoothedPosition() * Math.PI * 2.0 - Constants.ArmConstants.UpwardsAngle);
   }
 
   @Override

diff --git a/src/test/README.md b/src/test/README.md
@@ -0,0 +1 @@
+The unit test directory for this Gradle project. Uses JUnit as a testing framework.
diff --git a/src/test/java/frc/robot/commands/Positions/SpeakerShotTest.java b/src/test/java/frc/robot/commands/Positions/SpeakerShotTest.java
@@ -0,0 +1,89 @@
+package frc.robot.commands.Positions;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import frc.robot.Constants.ArmConstants;
+import java.util.Optional;
+import java.util.stream.DoubleStream;
+import java.util.stream.IntStream;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.DisplayName;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class SpeakerShotTest {
+
+  @Test
+  @DisplayName("ReasonableA,B,C Values")
+  void reasonableValues() {
+    assertTrue(
+        -ArmConstants.QUADRATIC_B * Math.pow((2 * ArmConstants.QUADRATIC_A), -1) > 0); // -b/2a
+  }
+
+  @ParameterizedTest(name = "Known Points Are Valid Points")
+  @MethodSource("knownPointsProvider")
+  void knownPointsCheck(double speakerDistance, double expectedResult) {
+    double reasonableError = 0.01;
+    double calculatedValue =
+        SpeakerShot.getRequiredAngle(Optional.of(speakerDistance)).get().doubleValue();
+    assertEquals(
+        expectedResult,
+        calculatedValue,
+        reasonableError,
+        () -> calculatedValue + " should equal " + expectedResult);
+  }
+
+  /** Returns a stream of arguments of known (distance, angle) pairs. */
+  static Stream<Arguments> knownPointsProvider() {
+    return Stream.of(
+        Arguments.of(1.27, ArmConstants.ShootCloseAngle),
+        Arguments.of(2.032, ArmConstants.ShootMediumAngle),
+        Arguments.of(2.794, 0.922), // Fake numbers
+        Arguments.of(3, 0.89), // Fake numbers
+        Arguments.of(4, 0.51)); // Fake numbers
+  }
+
+  @ParameterizedTest(name = "Inside Allowed Range")
+  @MethodSource("speakerRangeProvider")
+  void InsideAllowedRange(double doubleStream) {
+    assertTrue(
+        Math.abs(doubleStream) < ArmConstants.UpwardsAngle
+            && Math.abs(doubleStream) > ArmConstants.FloorPickupAngle);
+  }
+
+  /**
+   * Returns a range of double values that represent possible realistic-ish distances from the
+   * speaker
+   */
+  static DoubleStream speakerRangeProvider() {
+    getMaxPossibleRange();
+    IntStream intStream = IntStream.range(1, (int) getMaxPossibleRange() + 1);
+    DoubleStream doubleStream =
+        intStream.mapToDouble(
+            (number) -> {
+              System.out.println(((double) (number)) / 8);
+              return ((double) (number)) / 8;
+            });
+    return doubleStream;
+  }
+
+  /** Solves the quadratic equation for the larger number */
+  static double getMaxPossibleRange() {
+    double solution1 =
+        (-ArmConstants.QUADRATIC_B
+                - Math.sqrt(
+                    Math.pow(ArmConstants.QUADRATIC_B, 2)
+                        - 4 * ArmConstants.QUADRATIC_A * ArmConstants.QUADRATIC_C))
+            * Math.pow(2 * ArmConstants.QUADRATIC_A, -1);
+    double solution2 =
+        (-ArmConstants.QUADRATIC_B
+                + Math.sqrt(
+                    Math.pow(ArmConstants.QUADRATIC_B, 2)
+                        - 4 * ArmConstants.QUADRATIC_A * ArmConstants.QUADRATIC_C))
+            * Math.pow(2 * ArmConstants.QUADRATIC_A, -1);
+    return Math.max(solution1, solution2);
+  }
+}