Added getArmAngleFromDistance method

src/main/java/competition/subsystems/arm/ArmSubsystem.java

@@ -39,6 +39,7 @@ public class ArmSubsystem extends BaseSetpointSubsystem<Double> implements DataF
     public DoubleProperty softUpperLimit;
     public DoubleProperty softLowerLimit;
     public DoubleProperty speedLimitForNotCalibrated;
+    public DoubleProperty angleTrim;
     boolean hasCalibratedLeft;
     boolean hasCalibratedRight;
 
@@ -78,15 +79,17 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
         armPowerMax = pf.createPersistentProperty("ArmPowerMax", 0.5);
         armPowerMin = pf.createPersistentProperty("ArmPowerMin", -0.5);
 
-        // All the DoubleProperties below needs configuration.
-        ticksToMmRatio = pf.createPersistentProperty("TicksToDistanceRatio", 1000);
+        // ticksToMmRatio and armMotorRevLimit needs configuration
+        ticksToMmRatio = pf.createPersistentProperty("TicksToArmMmRatio", 1000);
+        armMotorRevolutionLimit = pf.createPersistentProperty("ArmMotorPositionLimit", 15000);
+
+        angleTrim = pf.createPersistentProperty("AngleTrim", 0);
 
         armMotorLeftRevolutionOffset = pf.createPersistentProperty(
                 "ArmMotorLeftRevolutionOffset", 0);
         armMotorRightRevolutionOffset = pf.createPersistentProperty(
                 "ArmMotorRightRevolutionOffset", 0);
 
-        armMotorRevolutionLimit = pf.createPersistentProperty("ArmMotorPositionLimit", 15000);
         softLowerLimit = pf.createPersistentProperty(
                 "SoftLowerLimit", armMotorRevolutionLimit.get() * 0.15);
         softUpperLimit = pf.createPersistentProperty(
@@ -193,7 +196,7 @@ public void stop() {
     }
 
     // TO-DO
-    public double convertTicksToDistance(double ticks) {
+    public double convertTicksToMm(double ticks) {
         return ticksToMmRatio.get() * ticks;
     }
 
@@ -207,6 +210,11 @@ public double convertShooterAngleToTicks(double angle) {
         return 0;
     }
 
+    public double getArmAngleFromDistance(double distanceFromSpeaker) {
+        // Distance: Inches; Angle: Degrees; Distance = Measured Distance - Calibration Offset
+        return (0.0019 * Math.pow(distanceFromSpeaker, 2) + (-0.7106 * distanceFromSpeaker) + 82.844) + angleTrim.get();
+    }
+
 
     // Returns an angle for the shooter that can be converted into arm position later if needed
     public double getUsefulArmPositionAngle(UsefulArmPosition usefulArmPosition) {
@@ -247,9 +255,9 @@ public LimitState getLimitState(XCANSparkMax motor) {
     public void armEncoderTicksUpdate() {
         aKitLog.record("ArmMotorLeftTicks", armMotorLeft.getPosition());
         aKitLog.record("ArmMotorRightTicks", armMotorRight.getPosition());
-        aKitLog.record("ArmMotorLeftDistance", convertTicksToDistance(
+        aKitLog.record("ArmMotorLeftMm", convertTicksToMm(
                 armMotorLeft.getPosition() + armMotorLeftRevolutionOffset.get()));
-        aKitLog.record("ArmMotorRightDistance", convertTicksToDistance(
+        aKitLog.record("ArmMotorRightMm", convertTicksToMm(
                 armMotorRight.getPosition() + armMotorRightRevolutionOffset.get()));
 
         aKitLog.record("ArmMotorToShooterAngle", convertTicksToShooterAngle(

src/test/java/competition/subsystems/arm/ArmSubsystemTest.java

@@ -1,6 +1,5 @@
 package competition.subsystems.arm;
 
-import com.revrobotics.SparkLimitSwitch;
 import competition.BaseCompetitionTest;
 import org.junit.Test;
 import xbot.common.controls.actuators.mock_adapters.MockCANSparkMax;
@@ -9,6 +8,7 @@
 import static org.junit.Assert.assertFalse;
 import static org.junit.Assert.assertNotEquals;
 import static org.junit.Assert.assertTrue;
+// Expect, Actual
 
 public class ArmSubsystemTest extends BaseCompetitionTest {
 
@@ -164,4 +164,14 @@ public void testLimits() {
         arm.setPower(-0.3);
         checkMotorPower(0);
     }
+
+
+    @Test
+    public void testGetArmAngleFromDistance() {
+        // In case if getArmAngleFromDistance for whatever reason is wayyyyyy off
+        // (Will likely fail is the equation is changed)
+        assertEquals(52, arm.getArmAngleFromDistance(50), 5);
+        assertEquals(31, arm.getArmAngleFromDistance(100), 5);
+        assertEquals(20, arm.getArmAngleFromDistance(150), 5);
+    }
 }