Nh math utils

src/main/java/org/usfirst/frc4048/common/Constants.java

@@ -23,4 +23,7 @@ public class Constants {
     public static final double VISION_CONSISTANCY_THRESHOLD = 0.25;
     public static final long MAX_LOG_TIME_WAIT = 10;
     public static final int LIGHTSTRIP_PORT = 0;
+    public static final double GRAVITY = -9.8;
+    public static final double SHOOTER_VELOCITY = 10.56;//m/s @ 5500rpm, 3500rpm // 8.9 or 8.1 or 10.65 or
+    public static final double RAMP_FROM_CENTER = 0.17;
 }

src/main/java/org/usfirst/frc4048/common/util/math/ArrayUtils.java

@@ -0,0 +1,73 @@
+package org.usfirst.frc4048.common.util.math;
+
+import java.lang.reflect.Array;
+import java.util.Arrays;
+import java.util.List;
+
+public class ArrayUtils {
+    public static <T extends Number> boolean contains(T[] array, T value){
+        for (T t : array) {
+            if (t.equals(value)) {
+                return true;
+            }
+        }
+        return false;
+    }
+    public static boolean contains(double[] array, double value){
+        for (double d : array){
+            if (d == value){
+                return true;
+            }
+        }
+        return false;
+    }
+    public static boolean contains(int[] array, int value){
+        for (int d : array){
+            if (d == value){
+                return true;
+            }
+        }
+        return false;
+    }
+    public static boolean allMatch(double[] array, double value){
+        for (double d : array){
+            if (d != value){
+                return false;
+            }
+        }
+        return true;
+    }
+    public static boolean allMatch(int[] array, int value){
+        for (int d : array){
+            if (d != value){
+                return false;
+            }
+        }
+        return true;
+    }
+    public static <T extends Number> boolean allMatch(T[] array, T value){
+        for (T t : array) {
+            if (!t.equals(value)) {
+                return false;
+            }
+        }
+        return true;
+    }
+    @SuppressWarnings("unchecked")
+    public static <T> T[] toArray(List<T> list, Class<T> tClass){
+        T[] t = (T[]) Array.newInstance(tClass, list.size());
+        return list.toArray(t);
+    }
+
+    public static double[] unwrap(Double[] a){
+        return Arrays.stream(a).mapToDouble(Double::doubleValue).toArray();
+    }
+
+    public static int[] unwrap(Integer[] a){
+        return Arrays.stream(a).mapToInt(Integer::intValue).toArray();
+    }
+
+    public static long[] unwrap(Long[] a){
+        return Arrays.stream(a).mapToLong(Long::longValue).toArray();
+    }
+}

src/main/java/org/usfirst/frc4048/common/util/math/PoseUtils.java

@@ -0,0 +1,182 @@
+package org.usfirst.frc4048.common.util.math;
+
+import edu.wpi.first.math.geometry.*;
+
+public class PoseUtils {
+    /**
+     * Estimates a future position based on the current position and velocities
+     *
+     * @param pose current pose
+     * @param vx   x velocity
+     * @param vy   y velocity
+     * @param time time over which velocity is applied
+     * @return Estimated position
+     */
+    public static Pose2d getFieldEstimatedFuturePose(Pose2d pose, double vx, double vy, double time) {
+        return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time);
+    }
+
+    /**
+     * Estimates a future position based on the current position and velocities
+     *
+     * @param pose current pose
+     * @param vx   x velocity
+     * @param vy   y velocity
+     * @param time time over which velocity is applied
+     * @return Estimated position
+     */
+    public static Transform2d getFieldEstimatedFuturePose(Transform2d pose, double vx, double vy, double time) {
+        return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time);
+    }
+
+    /**
+     * Estimates a future position based on the current position and velocities
+     *
+     * @param pose current pose
+     * @param vx   x velocity
+     * @param vy   y velocity
+     * @param time time over which velocity is applied
+     * @return Estimated position
+     */
+    public static Translation2d getFieldEstimatedFuturePose(Translation2d pose, double vx, double vy, double time) {
+        return pose.plus(new Translation2d(vx * time, vy * time));
+    }
+
+    /**
+     * Estimates a future position based on the current position and velocities
+     *
+     * @param pose current pose
+     * @param vx   x velocity
+     * @param vy   y velocity
+     * @param vz   z velocity
+     * @param time time over which velocity is applied
+     * @return Estimated position
+     */
+    public static Pose3d getFieldEstimatedFuturePose(Pose3d pose, double vx, double vy, double vz, double time) {
+        return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time);
+    }
+
+    /**
+     * Estimates a future position based on the current position and velocities
+     *
+     * @param pose current pose
+     * @param vx   x velocity
+     * @param vy   y velocity
+     * @param vz   z velocity
+     * @param time time over which velocity is applied
+     * @return Estimated position
+     */
+    public static Transform3d getFieldEstimatedFuturePose(Transform3d pose, double vx, double vy, double vz, double time) {
+        return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time);
+    }
+
+    /**
+     * Estimates a future position based on the current position and velocities
+     *
+     * @param pose current pose
+     * @param vx   x velocity
+     * @param vy   y velocity
+     * @param vz   z velocity
+     * @param time time over which velocity is applied
+     * @return Estimated position
+     */
+    public static Translation3d getFieldEstimatedFuturePose(Translation3d pose, double vx, double vy, double vz, double time) {
+        return pose.plus(new Translation3d(vx * time, vy * time, vz * time));
+    }
+
+    /**
+     * Estimates a future position based on the current position and velocities
+     *
+     * @param pose current pose
+     * @param vx   x velocity
+     * @param vy   y velocity
+     * @param vYaw yaw velocity
+     * @param time time over which velocity is applied
+     * @return Estimated position
+     */
+    public static Pose2d getFieldEstimatedFuturePose(Pose2d pose, double vx, double vy, double vYaw, double time) {
+        return pose.transformBy(new Transform2d(vx * time, vy * time, new Rotation2d(vYaw * time)));
+    }
+
+    /**
+     * Estimates a future position based on the current position and velocities
+     *
+     * @param pose current pose
+     * @param vx   x velocity
+     * @param vy   y velocity
+     * @param vYaw yaw velocity
+     * @param time time over which velocity is applied
+     * @return Estimated position
+     */
+    public static Transform2d getFieldEstimatedFuturePose(Transform2d pose, double vx, double vy, double vYaw, double time) {
+        return pose.plus(new Transform2d(vx * time, vy * time, Rotation2d.fromDegrees(vYaw * time)));
+    }
+
+    /**
+     * Estimates a future position based on the current position and velocities
+     *
+     * @param pose   current pose
+     * @param vx     x velocity
+     * @param vy     y velocity
+     * @param vz     z velocity
+     * @param vYaw   yaw velocity
+     * @param vRoll  roll velocity
+     * @param vPitch pitch velocity
+     * @param time   time over which velocity is applied
+     * @return Estimated position
+     */
+    public static Pose3d getFieldEstimatedFuturePose(Pose3d pose, double vx, double vy, double vz, double vYaw, double vRoll, double vPitch, double time) {
+        return pose.transformBy(new Transform3d(vx * time, vy * time, vz * time, new Rotation3d(vRoll * time, vPitch * time, vYaw * time)));
+    }
+
+    /**
+     * Estimates a future position based on the current position and velocities
+     *
+     * @param pose   current pose
+     * @param vx     x velocity
+     * @param vy     y velocity
+     * @param vz     z velocity
+     * @param vYaw   yaw velocity
+     * @param vRoll  roll velocity
+     * @param vPitch pitch velocity
+     * @param time   time over which velocity is applied
+     * @return Estimated position
+     */
+    public static Transform3d getFieldEstimatedFuturePose(Transform3d pose, double vx, double vy, double vz, double vYaw, double vRoll, double vPitch, double time) {
+        return pose.plus(new Transform3d(vx * time, vy * time, vz * time, new Rotation3d(vRoll * time, vPitch * time, vYaw * time)));
+    }
+
+
+    /**
+     * @param pose to modify
+     * @param z to add
+     * @param pitch to add
+     * @param roll to add
+     * @return 3D pose that inherits 2D attributes
+     */
+    public static Pose3d addDimension(Pose2d pose, double z, double pitch, double roll) {
+        return new Pose3d(addDimension(pose.getTranslation(), z), addDimension(pose.getRotation(), pitch, roll));
+    }
+
+    /**
+     * @param pose to modify
+     * @param z to add
+     * @return 3D pose that inherits 2D attributes
+     */
+    public static Translation3d addDimension(Translation2d pose, double z) {
+        return new Translation3d(pose.getX(), pose.getY(), z);
+    }
+
+    /**
+     * @param rotation to modify
+     * @param pitch to add
+     * @param roll to add
+     * @return 3D rotation that inherits yaw of 2D rotation
+     */
+    public static Rotation3d addDimension(Rotation2d rotation, double pitch, double roll) {
+        return new Rotation3d(roll, pitch, rotation.getRadians());
+    }
+    public static double slope(Translation2d startPose, Translation2d endPose){
+        return (endPose.getY() - startPose.getY()) / (endPose.getX() - startPose.getX());
+    }
+}

src/main/java/org/usfirst/frc4048/common/util/math/RangeUtils.java

@@ -0,0 +1,9 @@
+package org.usfirst.frc4048.common.util.math;
+
+public class RangeUtils {
+    public static double map(double value, double startMin, double startMax, double endMin, double endMax){
+        double endRange = endMax - endMin;
+        double startRange = startMax - startMin;
+        return ((value - startMin) * endRange / startRange) + endMin;
+    }
+}