From 6017fa52ff6f0babe1daf82be20311097053283e Mon Sep 17 00:00:00 2001 From: mebrahimaleem Date: Sat, 26 Oct 2024 10:32:46 -0700 Subject: [PATCH 1/4] update WPILib --- build.gradle | 2 +- vendordeps/PathplannerLib.json | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/build.gradle b/build.gradle index 07feb12..2eac5c7 100644 --- a/build.gradle +++ b/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 { diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index cae1363..6dc648d 100644 --- a/vendordeps/PathplannerLib.json +++ b/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,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.6" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.6", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 222b736ee50af03b17f2328da62d17d754d62a81 Mon Sep 17 00:00:00 2001 From: mebrahimaleem Date: Sat, 26 Oct 2024 14:42:51 -0700 Subject: [PATCH 2/4] Implemented most of FastPoly --- networktables.json | 1 + simgui.json | 9 ++ src/main/java/frc/robot/utils/FastPoly.java | 104 ++++++++++++++++++++ 3 files changed, 114 insertions(+) create mode 100644 networktables.json create mode 100644 src/main/java/frc/robot/utils/FastPoly.java diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui.json b/simgui.json index 0f24e0c..9dc2a59 100644 --- a/simgui.json +++ b/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 } } diff --git a/src/main/java/frc/robot/utils/FastPoly.java b/src/main/java/frc/robot/utils/FastPoly.java new file mode 100644 index 0000000..096fdc3 --- /dev/null +++ b/src/main/java/frc/robot/utils/FastPoly.java @@ -0,0 +1,104 @@ +package frc.robot.utils; + +//TODO: add docs + +public class FastPoly { + public class Point { + public final double x; + public final double y; + + public Point(double x, double y) { + this.x = x; + this.y = y; + } + + public double cdsq(Point tar) { + return Math.pow(tar.x - x, 2) + Math.pow(tar.y - y, 2); + } + } + + private enum Direction { + DIRECTION_CW, + DIRECTION_CCW + } + + private final int sides; + private final Point[] points; + private final double[] ltlims; + private final int[] ci = new int[2]; + + + public FastPoly(int sides, Point[] points, Point pos) { //NOTE: only convex polygons will work //TODO: enforce concave + this.sides = sides; + this.points = points; + 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); + if (!(exceedLim(ideal, ci[0], Direction.DIRECTION_CCW) || exceedLim(ideal, ci[1], Direction.DIRECTION_CW))) { + sk = false; + break; + } + } + if (sk) { + ci[0]++; + ci[1] = 0; + } + } + + private void updateLims(Point pos) { + final Point rbpxo = new Point(pos.x + 1, pos.y); + 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) { + + return new Point(0, 0); //TODO: implement + } + + private boolean exceedLim(Point pos, int i, Direction dir) { + + return false; //TODO: implement using ltlims + } + + // get required acceleration + public double calc(Point pos, double vx, double vy) { //NOTE: velocities are field relative + Point ideal = solveCI(pos); + updateLims(pos); + + if (exceedLim(ideal, ci[0], Direction.DIRECTION_CCW)) { + do { + ci[0]--; + if (ci[0] == -1) ci[0] = sides-1; + ideal = solveCI(pos); + } while (exceedLim(ideal, ci[0], Direction.DIRECTION_CCW)); + ci[1] = ci[0]+1; + if (ci[1] == sides) ci[1] = 0; + } + else if (exceedLim(ideal, ci[1], Direction.DIRECTION_CW)) { + do { + ci[1]++; + if (ci[1] == sides) ci[1] = 0; + ideal = solveCI(pos); + } while (exceedLim(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)))); + } +} \ No newline at end of file From c428f8feb876ec3fe76c544bf0383c144a7b2d82 Mon Sep 17 00:00:00 2001 From: Ebrahim Aleem Date: Mon, 28 Oct 2024 20:42:05 -0700 Subject: [PATCH 3/4] Implemented solveCI and exceedLim --- src/main/java/frc/robot/utils/FastPoly.java | 53 +++++++++++++++------ 1 file changed, 38 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/utils/FastPoly.java b/src/main/java/frc/robot/utils/FastPoly.java index 096fdc3..d1d0150 100644 --- a/src/main/java/frc/robot/utils/FastPoly.java +++ b/src/main/java/frc/robot/utils/FastPoly.java @@ -28,7 +28,7 @@ private enum Direction { private final int[] ci = new int[2]; - public FastPoly(int sides, Point[] points, Point pos) { //NOTE: only convex polygons will work //TODO: enforce concave + public FastPoly(int sides, Point[] points, Point pos, double vx, double vy) { //NOTE: only convex polygons will work //TODO: enforce convex this.sides = sides; this.points = points; ltlims = new double[sides]; @@ -41,8 +41,8 @@ public FastPoly(int sides, Point[] points, Point pos) { //NOTE: only convex poly for (int i = 0; i < sides-1; i++) { ci[0] = i; ci[1] = i+1; - ideal = solveCI(pos); - if (!(exceedLim(ideal, ci[0], Direction.DIRECTION_CCW) || exceedLim(ideal, ci[1], Direction.DIRECTION_CW))) { + 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; } @@ -65,36 +65,59 @@ private void updateLims(Point pos) { } } - private Point solveCI(Point pos) { + private Point solveCI(Point pos, double vx, double vy) { + 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); + } - return new Point(0, 0); //TODO: implement - } + final double x2 = points[ci[1]].x; + final double m1 = vx/vy; + final double m2 = (points[ci[0]].x - x2)/(y1 - y2); - private boolean exceedLim(Point pos, int i, Direction dir) { + final double ypmm1xp = pos.y - m1 * pos.x; - return false; //TODO: implement using ltlims + 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 required acceleration public double calc(Point pos, double vx, double vy) { //NOTE: velocities are field relative - Point ideal = solveCI(pos); + Point ideal = solveCI(pos, vx, vy); updateLims(pos); - if (exceedLim(ideal, ci[0], Direction.DIRECTION_CCW)) { + if (exceedLim(pos, ideal, ci[0], Direction.DIRECTION_CCW)) { do { ci[0]--; if (ci[0] == -1) ci[0] = sides-1; - ideal = solveCI(pos); - } while (exceedLim(ideal, ci[0], Direction.DIRECTION_CCW)); + 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(ideal, ci[1], Direction.DIRECTION_CW)) { + else if (exceedLim(pos, ideal, ci[1], Direction.DIRECTION_CW)) { do { ci[1]++; if (ci[1] == sides) ci[1] = 0; - ideal = solveCI(pos); - } while (exceedLim(ideal, ci[1], Direction.DIRECTION_CW)); + 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; } From ef3e390a892cf4c8f640838f3a0d4bc297c0e967 Mon Sep 17 00:00:00 2001 From: Ebrahim Aleem Date: Mon, 28 Oct 2024 21:55:03 -0700 Subject: [PATCH 4/4] Finished impl., needs refact. and simtesting --- .../frc/robot/subsystems/DriveSubsystem.java | 21 ++++++ src/main/java/frc/robot/utils/FastPoly.java | 67 ++++++++++++++++--- 2 files changed, 78 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index f606d5b..10c1e69 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/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( diff --git a/src/main/java/frc/robot/utils/FastPoly.java b/src/main/java/frc/robot/utils/FastPoly.java index d1d0150..00007ff 100644 --- a/src/main/java/frc/robot/utils/FastPoly.java +++ b/src/main/java/frc/robot/utils/FastPoly.java @@ -1,8 +1,15 @@ 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; @@ -12,11 +19,21 @@ public Point(double x, double y) { 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 @@ -24,16 +41,26 @@ private enum Direction { private final int sides; private final Point[] points; - private final double[] ltlims; - private final int[] ci = new int[2]; + 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); + } - - public FastPoly(int sides, Point[] points, Point pos, double vx, double vy) { //NOTE: only convex polygons will work //TODO: enforce convex - this.sides = sides; - this.points = points; + //TODO: allow zero vx vy on init by skipping ci calculations (then remove the other constructor) + private FastPoly(Pair 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; @@ -53,8 +80,21 @@ public FastPoly(int sides, Point[] points, Point pos, double vx, double vy) { // } } + private static Pair GenPoly(PolySource source) { + switch (source) { + case POLY_SOURCE_TEST_SQUARE: + default: + return new Pair(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); + 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 @@ -66,6 +106,7 @@ private void updateLims(Point pos) { } 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 @@ -98,7 +139,7 @@ private boolean exceedLim(Point pos, Point sol, int i, Direction dir) { return lim < ltlims[i]; } - // get required acceleration + // 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); @@ -122,6 +163,12 @@ else if (exceedLim(pos, ideal, ci[1], Direction.DIRECTION_CW)) { 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)))); + 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)]; } } \ No newline at end of file