diff --git a/src/main/java/frc/robot/tests/WheelsCalibrationCTRE.java b/src/main/java/frc/robot/tests/WheelsCalibrationCTRE.java index 314435a..2858f9c 100644 --- a/src/main/java/frc/robot/tests/WheelsCalibrationCTRE.java +++ b/src/main/java/frc/robot/tests/WheelsCalibrationCTRE.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.Constants; import frc.robot.utils.Config.MapleConfigFile; -import frc.robot.utils.Math.AngleHelpers; +import frc.robot.utils.MapleMaths.Angles; import static frc.robot.Constants.WheelCalibrationConfigs.WheelToBeCalibrated; @@ -68,7 +68,7 @@ public void testPeriodic() { } private double getCanCoderReadingRadian(CANcoder canCoder) { - return AngleHelpers.simplifyAngle(canCoder.getAbsolutePosition().getValue() * Math.PI * 2); + return Angles.simplifyAngle(canCoder.getAbsolutePosition().getValue() * Math.PI * 2); } private void writeConfigurationFile() { diff --git a/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java b/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java index 2c40ae4..bacaa78 100644 --- a/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java +++ b/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; -import frc.robot.utils.Math.LookUpTable; +import frc.robot.utils.MapleMaths.CommonMath; import java.util.function.DoubleSupplier; @@ -71,7 +71,7 @@ public double getRotationalSpeedFromJoystick(double maxAngularVelocityRadPerSec) * @param otherAxisValue the value of the other axis on the stick * */ private static double applySmartDeadBand(double axisValue, double otherAxisValue) { - final double deadBand = LookUpTable.linearInterpretationWithBounding( + final double deadBand = CommonMath.linearInterpretationWithBounding( 0, Constants.DriveConfigs.deadBandWhenOtherAxisEmpty, 1, Constants.DriveConfigs.deadBandWhenOtherAxisFull, Math.abs(otherAxisValue) diff --git a/src/main/java/frc/robot/utils/Math/AngleHelpers.java b/src/main/java/frc/robot/utils/MapleMaths/Angles.java similarity index 96% rename from src/main/java/frc/robot/utils/Math/AngleHelpers.java rename to src/main/java/frc/robot/utils/MapleMaths/Angles.java index f028db6..70dde5b 100644 --- a/src/main/java/frc/robot/utils/Math/AngleHelpers.java +++ b/src/main/java/frc/robot/utils/MapleMaths/Angles.java @@ -1,6 +1,6 @@ -package frc.robot.utils.Math; +package frc.robot.utils.MapleMaths; -public class AngleHelpers { +public class Angles { /** * simplify an angle into the range 0-360 degrees * diff --git a/src/main/java/frc/robot/utils/Math/BezierCurve.java b/src/main/java/frc/robot/utils/MapleMaths/BezierCurve.java similarity index 98% rename from src/main/java/frc/robot/utils/Math/BezierCurve.java rename to src/main/java/frc/robot/utils/MapleMaths/BezierCurve.java index 862aa15..72d36c7 100644 --- a/src/main/java/frc/robot/utils/Math/BezierCurve.java +++ b/src/main/java/frc/robot/utils/MapleMaths/BezierCurve.java @@ -1,4 +1,4 @@ -package frc.robot.utils.Math; // package frc.robot.Helpers.MathHelpers; +package frc.robot.utils.MapleMaths; // package frc.robot.Helpers.MathHelpers; // /// ** // * Bézier curve with four points diff --git a/src/main/java/frc/robot/utils/MapleMaths/CommonMath.java b/src/main/java/frc/robot/utils/MapleMaths/CommonMath.java new file mode 100644 index 0000000..786ba60 --- /dev/null +++ b/src/main/java/frc/robot/utils/MapleMaths/CommonMath.java @@ -0,0 +1,11 @@ +package frc.robot.utils.MapleMaths; + +public class CommonMath { + public static double linearInterpretationWithBounding(double x1, double y1, double x2, double y2, double x) { + final double minX = Math.min(x1, x2), maxX = Math.max(x1, x2); + return linearInterpretation(x1, y1, x2, y2, Math.min(maxX, Math.max(minX, x))); + } + public static double linearInterpretation(double x1, double y1, double x2, double y2, double x) { + return y1 + (x - x1) * (y2 - y1) / (x2 - x1); + } +} diff --git a/src/main/java/frc/robot/utils/Math/SpeedCurves.java b/src/main/java/frc/robot/utils/MapleMaths/SpeedCurves.java similarity index 96% rename from src/main/java/frc/robot/utils/Math/SpeedCurves.java rename to src/main/java/frc/robot/utils/MapleMaths/SpeedCurves.java index 83ff3a3..58ddf44 100644 --- a/src/main/java/frc/robot/utils/Math/SpeedCurves.java +++ b/src/main/java/frc/robot/utils/MapleMaths/SpeedCurves.java @@ -1,4 +1,4 @@ -package frc.robot.utils.Math; // package frc.robot.Helpers.MathHelpers; +package frc.robot.utils.MapleMaths; // package frc.robot.Helpers.MathHelpers; // /// * // * https://developer.mozilla.org/en-US/docs/Web/CSS/easing-function diff --git a/src/main/java/frc/robot/utils/Math/StatisticsHelpers.java b/src/main/java/frc/robot/utils/MapleMaths/Statistics.java similarity index 97% rename from src/main/java/frc/robot/utils/Math/StatisticsHelpers.java rename to src/main/java/frc/robot/utils/MapleMaths/Statistics.java index 751a591..712100d 100644 --- a/src/main/java/frc/robot/utils/Math/StatisticsHelpers.java +++ b/src/main/java/frc/robot/utils/MapleMaths/Statistics.java @@ -1,6 +1,6 @@ -package frc.robot.utils.Math; +package frc.robot.utils.MapleMaths; -public final class StatisticsHelpers { +public final class Statistics { public static double getMean(double[] dataSet) { double sum = 0; for (double data : dataSet) sum += data; diff --git a/src/main/java/frc/robot/utils/Math/LookUpTable.java b/src/main/java/frc/robot/utils/Math/LookUpTable.java deleted file mode 100644 index c0430f7..0000000 --- a/src/main/java/frc/robot/utils/Math/LookUpTable.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.robot.utils.Math; - -public class LookUpTable { - public final double[] xValues; - public final double[] yValues; - final int n; - - public LookUpTable(double[] xValues, double[] yValues) { - if (xValues.length != yValues.length) - throw new IllegalArgumentException("look up table length not match"); - double prev_x = -Double.POSITIVE_INFINITY; - for (double x : xValues) { - if (x < prev_x) - throw new IllegalArgumentException("look up table X must be in increasing order"); - prev_x = x; - } - this.xValues = xValues; - this.yValues = yValues; - this.n = xValues.length; - } - - public double getYPrediction(double x) { - if (x < xValues[0]) return yValues[0]; - for (int i = 0; i < n - 1; i++) - if (xValues[i] < x && x < xValues[i + 1]) - return linearInterpretation(xValues[i], yValues[i], xValues[i + 1], yValues[i + 1], x); - return yValues[yValues.length - 1]; - } - - public static double linearInterpretationWithBounding( - double x1, double y1, double x2, double y2, double x) { - final double minX = Math.min(x1, x2), maxX = Math.max(x1, x2); - return linearInterpretation(x1, y1, x2, y2, Math.min(maxX, Math.max(minX, x))); - } - - public static double linearInterpretation(double x1, double y1, double x2, double y2, double x) { - return y1 + (x - x1) * (y2 - y1) / (x2 - x1); - } -} diff --git a/src/main/java/frc/robot/utils/MechanismControl/MapleSimplePIDController.java b/src/main/java/frc/robot/utils/MechanismControl/MapleSimplePIDController.java index f721982..f148201 100644 --- a/src/main/java/frc/robot/utils/MechanismControl/MapleSimplePIDController.java +++ b/src/main/java/frc/robot/utils/MechanismControl/MapleSimplePIDController.java @@ -1,7 +1,6 @@ package frc.robot.utils.MechanismControl; -import frc.robot.utils.Math.AngleHelpers; -import frc.robot.utils.Math.LookUpTable; +import frc.robot.utils.MapleMaths.Angles; import org.littletonrobotics.junction.Logger; /** @@ -32,7 +31,7 @@ public double getMotorPower(double mechanismVelocity, double mechanismPosition) mechanismPosition + mechanismVelocity * profile.mechanismDecelerationTime, error = profile.isMechanismInCycle - ? AngleHelpers.getActualDifference(mechanismPositionWithDerivative, desiredPosition) + ? Angles.getActualDifference(mechanismPositionWithDerivative, desiredPosition) : desiredPosition - mechanismPositionWithDerivative; if (Math.abs(error) < profile.errorTolerance) return 0; final double