Boundaries

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.2.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.2"
 }
 
 java {

networktables.json

@@ -0,0 +1 @@
+[]

simgui.json

@@ -13,6 +13,12 @@
     },
     "windows": {
       "/SmartDashboard/Field": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
         "window": {
           "visible": true
         }
@@ -28,5 +34,8 @@
         "open": true
       }
     }
+  },
+  "NetworkTables Info": {
+    "visible": true
   }
 }

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -21,6 +21,8 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.VisionConstants;
+import frc.robot.utils.FastPoly;
+import frc.robot.utils.FastPoly.PolySource;
 import frc.robot.Robot;
 
 public class DriveSubsystem extends SubsystemBase {
@@ -71,11 +73,15 @@ public class DriveSubsystem extends SubsystemBase {
 
   private final Field2d m_field = new Field2d();
 
+  private final FastPoly m_polySolver;
+
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
     SmartDashboard.putData("Field", m_field);
     m_headingCorrectionTimer.restart();
     m_headingCorrectionPID.enableContinuousInput(-Math.PI, Math.PI);
+
+    m_polySolver = new FastPoly(PolySource.POLY_SOURCE_TEST_SQUARE, FastPoly.Point.FromPose(getPose()));
   }
 
   @Override
@@ -162,7 +168,22 @@ public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRe
       // then maintain our desired angle
       calculatedRotation = m_headingCorrectionPID.calculate(currentAngle);
     }
+
+    final ChassisSpeeds robotRelative = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation,
+                Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle));
+
+    final double vx = fieldRelative ? xSpeed : robotRelative.vxMetersPerSecond;
+    final double vy = fieldRelative ? ySpeed : robotRelative.vyMetersPerSecond;
 
+    final double scale = m_polySolver.calc(FastPoly.Point.FromPose(getPose()), vx, vy);
+    final double inv = Math.sqrt(Math.pow(vx, 2) + Math.pow(vy, 2)); // For larger polygons (area wise), it may be better to instead compare with the square of scale and take the square root only when calculating the scalar
+
+    if (scale < inv) {
+      final double scalar = scale/inv;
+      xSpeed *= scalar;
+      ySpeed *= scalar;
+    }
+
     // Depending on whether the robot is being driven in field relative, calculate
     // the desired states for each of the modules
     SwerveModuleState[] swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(

src/main/java/frc/robot/utils/FastPoly.java

@@ -0,0 +1,174 @@
+package frc.robot.utils;
+
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.Robot;
+
+//TODO: add docs
+
+public class FastPoly {
+    //TODO: create a separate file for Point so that there does not need to be an encapsulation workaround, or make Point static (not sure if that will break code)
+    private static FastPoly dummy = new FastPoly();
+
+    public class Point {
+        public final double x;
+        public final double y;
+
+        public Point(double x, double y) {
+            this.x = x;
+            this.y = y;
+        }
+
+        //TODO: replace this with a typecast overload (if thats possible in java)
+        public static Point FromPose(Pose2d pose) {
+            return dummy.new Point(pose.getX(), pose.getY());
+        }
+
+        public double cdsq(Point tar) {
+            return Math.pow(tar.x - x, 2) + Math.pow(tar.y - y, 2);
+        }
+    }
+
+    //TODO: add file source
+    public enum PolySource {
+        POLY_SOURCE_TEST_SQUARE
+    }
+
+    private enum Direction {
+        DIRECTION_CW,
+        DIRECTION_CCW
+    }
+
+    private final int sides;
+    private final Point[] points;
+    private final double[] ltlims; // maximum valid angles for each point (inclusive)
+    private final int[] ci = new int[2]; // current relevant vertex indices
+
+    // Only used internally for dummy enclosing type
+    private FastPoly() {
+        ltlims = null;
+        points = null;
+        sides = 0;
+    }
+
+    public FastPoly(PolySource source, Point pos) {
+        this(GenPoly(source), pos, 1d, 1d); 
+    }
+
+    //TODO: allow zero vx vy on init by skipping ci calculations (then remove the other constructor)
+    private FastPoly(Pair<Integer, Point[]> poly, Point pos, double vx, double vy) { //NOTE: only convex polygons will work //TODO: enforce convex
+        this.sides = poly.getFirst();
+        this.points = poly.getSecond();
+        ltlims = new double[sides];
+
+        Point ideal;
+        boolean sk = true;
+
+        updateLims(pos);
+        for (int i = 0; i < sides-1; i++) {
+            ci[0] = i;
+            ci[1] = i+1;
+            ideal = solveCI(pos, vx, vy);
+            if (!(exceedLim(pos, ideal, ci[0], Direction.DIRECTION_CCW) || exceedLim(pos, ideal, ci[1], Direction.DIRECTION_CW))) {
+                sk = false;
+                break;
+            }
+        }
+        if (sk) {
+            ci[0]++;
+            ci[1] = 0;
+        }
+    }
+
+    private static Pair<Integer, Point[]> GenPoly(PolySource source) {
+        switch (source) {
+            case POLY_SOURCE_TEST_SQUARE:
+            default:
+                return new Pair<Integer,FastPoly.Point[]>(4, new Point[] {
+                    dummy.new Point(-1, 1),
+                    dummy.new Point(1, 1),
+                    dummy.new Point(1, -1),
+                    dummy.new Point(-1, -1)
+                });
+        }
+    }
+
+    private void updateLims(Point pos) {
+        final Point rbpxo = new Point(pos.x + 1, pos.y); //robot position with x offset (for easy law of cosines)
+        for (int i = 0; i < sides; i ++) {
+            //NOTE: theta bearing ref is +x
+            //NOTE: uses WPILib coordinate system
+
+            //TODO: find a faster way to calculate angle limits
+            final double bsq = pos.cdsq(points[i]);
+            ltlims[i] = Math.acos((rbpxo.cdsq(points[i]) - bsq - 1) / (2 * Math.sqrt(bsq)));
+        }
+    }
+
+    private Point solveCI(Point pos, double vx, double vy) {
+        //TODO: verify this math actually works
+        final double y1 = points[ci[0]].y;
+        final double y2 = points[ci[1]].y;
+        if (y1 == y2) { //m2 = infinity
+            final double m1 = vx/vy;
+            return new Point((y1 - pos.y + m1 * pos.x) / m1, y1);
+        }
+
+        final double x2 = points[ci[1]].x;
+        final double m1 = vx/vy;
+        final double m2 = (points[ci[0]].x - x2)/(y1 - y2);
+
+        final double ypmm1xp = pos.y - m1 * pos.x;
+
+        final double x = (m2 * x2 + ypmm1xp - y2) / (m2 - m1);
+        return new Point(x, x * m1 + ypmm1xp);
+
+        /*
+         *  -m1 1   -m1x1+y1
+         *  -m2 1   -m2x2+y2
+         */
+    }
+
+    private boolean exceedLim(Point pos, Point sol, int i, Direction dir) {
+        final Point rbpxo = new Point(pos.x + 1, pos.y);
+        final double bsq = pos.cdsq(sol);
+        final double lim = Math.acos((rbpxo.cdsq(sol) - bsq - 1) / (2 * Math.sqrt(bsq)));
+        if (dir == Direction.DIRECTION_CW) {
+            return lim >= ltlims[i];
+        }
+        return lim < ltlims[i];
+    }
+
+    // get absolute maximum velocity
+    public double calc(Point pos, double vx, double vy) { //NOTE: velocities are field relative
+        Point ideal = solveCI(pos, vx, vy);
+        updateLims(pos);
+
+        if (exceedLim(pos, ideal, ci[0], Direction.DIRECTION_CCW)) {
+            do {
+                ci[0]--;
+                if (ci[0] == -1) ci[0] = sides-1;
+                ideal = solveCI(pos, vx, vy);
+            } while (exceedLim(pos, ideal, ci[0], Direction.DIRECTION_CCW));
+            ci[1] = ci[0]+1;
+            if (ci[1] == sides) ci[1] = 0;
+        }
+        else if (exceedLim(pos, ideal, ci[1], Direction.DIRECTION_CW)) {
+            do {
+                ci[1]++;
+                if (ci[1] == sides) ci[1] = 0;
+                ideal = solveCI(pos, vx, vy);
+            } while (exceedLim(pos, ideal, ci[1], Direction.DIRECTION_CW));
+            ci[0] = ci[1]-1;
+            if (ci[0] == -1) ci[0] = sides-1;
+        }
+
+        return (Math.pow(vx, 2) + Math.pow(vy, 2) / (2 * Math.sqrt(Math.pow(ideal.x, 2) + Math.pow(ideal.y, 2)))) * Robot.kDefaultPeriod;
+    }
+
+    private static int[] sigmap = new int[] {1, -1};
+
+    public int FastSignum(double x) {
+        return sigmap[(int) ((Double.doubleToLongBits(x) & 0x8000000000000000L) >> 0x3F)];
+    }
+}

vendordeps/PathplannerLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.1.6",
+    "version": "2024.2.8",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,15 +12,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.1.6"
+            "version": "2024.2.8"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.1.6",
+            "version": "2024.2.8",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,