From db90499976e45cd51bba867344328df7be8913d7 Mon Sep 17 00:00:00 2001 From: Grady Whelan Date: Fri, 6 Dec 2019 15:22:29 -0800 Subject: [PATCH 01/25] Added implementation in Drive for pure pursuit Please add methods specific to your pure pursuit class --- .../team2813/frc2019/subsystems/Drive.java | 30 +++++++++++++++---- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/Drive.java b/OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/Drive.java index 33d7226..158fd57 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/Drive.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/Drive.java @@ -58,6 +58,7 @@ public class Drive extends Subsystem { private static double limelightDegrees = 0.0; // TODO: 10/05/2019 replace with actual Limelight angle private static final double ALLOWABLE_LIMELIGHT_ERROR = 0.0; // TODO: 10/05/2019 replace with actual allowable angle error private static final double MIN_AUTO_POS_CHANGE = 0.0; // TODO: 10/05/2019 tune + private static boolean purePursuit = false; // private static final double MIN_AUTO_SPEED_FPS = 0.33; // TODO: 10/05/2019 tune // private static final double MIN_AUTO_SPEED_ENCODER_TICKS = MIN_AUTO_SPEED_FPS * ENCODER_TICKS_PER_FOOT; @@ -154,11 +155,16 @@ protected void teleopControls_() throws CTREException, SparkMaxException { driveMode = DriveMode.OPEN_LOOP; teleopDrive(TELEOP_DRIVE_TYPE); } else { - driveMode = DriveMode.SMART_MOTION; - AUTO_BUTTON.whenPressed(() -> { - while (Math.abs(limelightDegrees) > ALLOWABLE_LIMELIGHT_ERROR) - autoDrive(limelightDegrees); - }); + if (purePursuit) { + setPurePursuitVelocities(); + } else { + startPurePursuit(); + } +// driveMode = DriveMode.SMART_MOTION; +// AUTO_BUTTON.whenPressed(() -> { +// while (Math.abs(limelightDegrees) > ALLOWABLE_LIMELIGHT_ERROR) +// autoDrive(limelightDegrees); +// }); } } @@ -193,6 +199,20 @@ public synchronized void setBrakeMode(boolean brake) { } } + + private void setPurePursuitVelocities() { + driveMode = DriveMode.VELOCITY; + // replace with pure pursuit method call to give enc values + // replace with pure pursuit method call to get and set left_demand + // replace with pure pursuit method call to get and set right_demand + // need to set purePursuit to false when finished + } + + private void startPurePursuit() { + purePursuit = true; + // add any code needed here to start pure pursuit + } + private enum DriveMode { OPEN_LOOP(ControlType.kDutyCycle), SMART_MOTION(ControlType.kSmartMotion), From 0a9d09f13f416c022574fa78dd64602918c1f63a Mon Sep 17 00:00:00 2001 From: saanvi25 <55930332+saanvi25@users.noreply.github.com> Date: Fri, 6 Dec 2019 16:25:09 -0800 Subject: [PATCH 02/25] Create purePursuit.java --- .../java/com/team2813/frc2019/subsystems/purePursuit.java | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/purePursuit.java diff --git a/OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/purePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/purePursuit.java new file mode 100644 index 0000000..532cac1 --- /dev/null +++ b/OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/purePursuit.java @@ -0,0 +1,8 @@ +package com.team2813.frc2019.subsystems; + + +public class purePursuit { + public void getStarterValue(double rightEncoderVal, double leftEncoderVal, double endPosition){ + + } +} From be8709a7b6ab5523fbc701ee755445aad0b6f8b3 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 6 Dec 2019 16:57:56 -0800 Subject: [PATCH 03/25] Update PurePursuit.java moved to lib --- .../purePursuit.java => lib/purePursuit/PurePursuit.java} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename OffSeasonBot2019/src/main/java/com/team2813/{frc2019/subsystems/purePursuit.java => lib/purePursuit/PurePursuit.java} (62%) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/purePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java similarity index 62% rename from OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/purePursuit.java rename to OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 532cac1..c6aed2a 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/purePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -1,7 +1,7 @@ -package com.team2813.frc2019.subsystems; +package com.team2813.lib.purePursuit; -public class purePursuit { +public class PurePursuit { public void getStarterValue(double rightEncoderVal, double leftEncoderVal, double endPosition){ } From 31193149a47237a29dd0ddc77cc4661f69d32122 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 6 Dec 2019 21:27:32 -0800 Subject: [PATCH 04/25] Update PurePursuit.java did most of the function to get the start location. just need to find out how to return x and y. also got rid of endPosition parameter, because was not needed in calculations of start location --- .../com/team2813/lib/purePursuit/PurePursuit.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index c6aed2a..dcdd8fb 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -2,7 +2,14 @@ public class PurePursuit { - public void getStarterValue(double rightEncoderVal, double leftEncoderVal, double endPosition){ - + public void getStarterValue(double rightEncoderVal, double leftEncoderVal){ + double currentRightVal = 0.0; + double currentLeftVal = 0.0; + double robot_angle = 0.0; //radians + double deltaRightVal = currentRightVal - rightEncoderVal; + double deltaLeftVal = currentLeftVal - leftEncoderVal; + double dist = (deltaRightVal + deltaLeftVal)/2; + double xVal = dist * Math.cos(robot_angle); + double yVal = dist * Math.sin(robot_angle); } } From 250e41f8c68ab6722b845ed85add49069575a3aa Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 6 Dec 2019 23:35:41 -0800 Subject: [PATCH 05/25] Update PurePursuit.java was finally able to make the method return the location --- .../com/team2813/lib/purePursuit/PurePursuit.java | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index dcdd8fb..315d552 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -1,15 +1,24 @@ package com.team2813.lib.purePursuit; +import java.awt.geom.Point2D; +import java.util.ArrayList; + +import static java.awt.geom.Point2D.*; + public class PurePursuit { - public void getStarterValue(double rightEncoderVal, double leftEncoderVal){ + public ArrayList getLocation(double rightEncoderVal, double leftEncoderVal, ArrayList coordinates) { double currentRightVal = 0.0; double currentLeftVal = 0.0; double robot_angle = 0.0; //radians + Point2D location = new Point2D.Double(); double deltaRightVal = currentRightVal - rightEncoderVal; double deltaLeftVal = currentLeftVal - leftEncoderVal; - double dist = (deltaRightVal + deltaLeftVal)/2; + double dist = (deltaRightVal + deltaLeftVal) / 2; double xVal = dist * Math.cos(robot_angle); double yVal = dist * Math.sin(robot_angle); + location.setLocation(xVal, yVal); + coordinates.add(location); + return coordinates; } } From 76954845af61ff605a99fc7f74718cb81df74e5f Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Sun, 8 Dec 2019 14:42:34 -0800 Subject: [PATCH 06/25] Update PurePursuit.java Created a method for injecting points, put some code in, still needs some work though --- .../com/team2813/lib/purePursuit/PurePursuit.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 315d552..e607ba4 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -7,7 +7,8 @@ import static java.awt.geom.Point2D.*; public class PurePursuit { - public ArrayList getLocation(double rightEncoderVal, double leftEncoderVal, ArrayList coordinates) { + // finds coordinate location of the robot + public Point2D getLocation(double rightEncoderVal, double leftEncoderVal) { double currentRightVal = 0.0; double currentLeftVal = 0.0; double robot_angle = 0.0; //radians @@ -18,7 +19,12 @@ public ArrayList getLocation(double rightEncoderVal, double leftEncoder double xVal = dist * Math.cos(robot_angle); double yVal = dist * Math.sin(robot_angle); location.setLocation(xVal, yVal); - coordinates.add(location); - return coordinates; + return location; + } + public ArrayList injectPoints(ArrayList path,Point2D startPosition, Point2D endPosition, double spacing){ + Point2D vector = new Point2D.Double(); + double xDif = endPosition.getX() - startPosition.getX(); + double yDif = endPosition.getY() - startPosition.getY(); + vector.setLocation(xDif, yDif); } } From f7f9a7cdf28d0cc922ad3d8cb4e213d925427347 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Sun, 8 Dec 2019 16:39:22 -0800 Subject: [PATCH 07/25] Update PurePursuit.java just making some changes to point injecter --- .../com/team2813/lib/purePursuit/PurePursuit.java | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index e607ba4..a61b2c1 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -21,10 +21,15 @@ public Point2D getLocation(double rightEncoderVal, double leftEncoderVal) { location.setLocation(xVal, yVal); return location; } - public ArrayList injectPoints(ArrayList path,Point2D startPosition, Point2D endPosition, double spacing){ - Point2D vector = new Point2D.Double(); - double xDif = endPosition.getX() - startPosition.getX(); - double yDif = endPosition.getY() - startPosition.getY(); - vector.setLocation(xDif, yDif); + //injects points + public ArrayList injectPoints(ArrayList path, double spacing){ + for(int i = 0; i < path.size(); i++){ + Point2D startPosition = path.get(i); + Point2D endPosition = path.get(i + 1); + double xDif = endPosition.getX() - startPosition.getX(); + double yDif = endPosition.getY() - startPosition.getY(); + Point2D vector = new Point2D.Double(); + vector.setLocation(xDif, yDif); + } } } From e840c070d045d3bd34d1e8c4b4fc10594da26f9c Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Mon, 9 Dec 2019 16:18:46 -0800 Subject: [PATCH 08/25] Update PurePursuit.java finished point injecter --- .../team2813/lib/purePursuit/PurePursuit.java | 29 ++++++++++++++----- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index a61b2c1..083655a 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -22,14 +22,29 @@ public Point2D getLocation(double rightEncoderVal, double leftEncoderVal) { return location; } //injects points - public ArrayList injectPoints(ArrayList path, double spacing){ + public ArrayList injectPoints(ArrayList path, double spacing) { + int numSegments = path.size() - 1; for(int i = 0; i < path.size(); i++){ - Point2D startPosition = path.get(i); - Point2D endPosition = path.get(i + 1); - double xDif = endPosition.getX() - startPosition.getX(); - double yDif = endPosition.getY() - startPosition.getY(); - Point2D vector = new Point2D.Double(); - vector.setLocation(xDif, yDif); + if(numSegments != 0){ + Point2D startPosition = path.get(i); + Point2D endPosition = path.get(i + 1); + double xDif = endPosition.getX() - startPosition.getX(); + double yDif = endPosition.getY() - startPosition.getY(); + Point2D vector = new Point2D.Double(); + vector.setLocation(xDif, yDif); + double vectorMagnitude = Math.sqrt((vector.getX() * vector.getX()) + (vector.getY() * vector.getY())); + double numPoints = Math.ceil(Math.sqrt(vectorMagnitude/spacing)); + Point2D unitVector = new Point2D.Double(); + unitVector.setLocation(xDif/vectorMagnitude * spacing, yDif/vectorMagnitude * spacing); + Point2D newPoint = new Point2D.Double(); + for(int j = 0; j < numPoints; j++){ + newPoint.setLocation(startPosition.getX() + unitVector.getX() * j, startPosition.getY() + unitVector.getY() * j); + path.add(i + j + 1, newPoint); + } + i += numPoints; + numSegments -= 1; + } } + return path; } } From 10192584f1be24803c775fefafd84f4065182e32 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Wed, 11 Dec 2019 15:11:56 -0800 Subject: [PATCH 09/25] Update PurePursuit.java created and completed smoother function --- .../team2813/lib/purePursuit/PurePursuit.java | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 083655a..508564b 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -47,4 +47,38 @@ public ArrayList injectPoints(ArrayList path, double spacing) } return path; } + public ArrayList smoother(ArrayList path, double a, double b, double tolerance){ + double change = tolerance; + double newX = 0.0; + double newY = 0.0; + double x; + double y; + ArrayList newPath = new ArrayList(); + for(int i = 0; i < path.size(); i++){ + newPath.add(path.get(i)); + } + while(change >= tolerance){ + change = 0.0; + for(int i = 1; i < path.size() - 1; i++){ + for(int j = 0; j < 2; j++){ + if(j == 0){ + x = path.get(i).getX(); + double aux = newPath.get(i).getX(); + newX = newPath.get(i).getX(); + newX += a * (x - aux) + b * (newPath.get(i - 1).getX() + newPath.get(i + 1).getX()) - (2.0 * aux); + change += Math.abs(aux - newX); + } + if(j == 1){ + y = path.get(i).getY(); + double aux = newPath.get(i).getY(); + newY = newPath.get(i).getY(); + newY += a * (y - aux) + b * (newPath.get(i - 1).getY() + newPath.get(i + 1).getY()) - (2.0 * aux); + change += Math.abs(aux - newY); + } + } + newPath.get(i).setLocation(newX, newY); + } + } + return newPath; + } } From abd2f061251d1e00dd95604f5917621b25362af3 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Wed, 11 Dec 2019 15:47:27 -0800 Subject: [PATCH 10/25] Update PurePursuit.java created and completed method that finds distance between points --- .../team2813/lib/purePursuit/PurePursuit.java | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 508564b..4151053 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -47,6 +47,7 @@ public ArrayList injectPoints(ArrayList path, double spacing) } return path; } + //smooths path public ArrayList smoother(ArrayList path, double a, double b, double tolerance){ double change = tolerance; double newX = 0.0; @@ -81,4 +82,19 @@ public ArrayList smoother(ArrayList path, double a, double b, } return newPath; } + //calculates the distance between points + public double[] distancePoints(ArrayList path){ + double xDif; + double yDif; + double[] distancePoints = new double[path.size()]; + xDif = path.get(0).getX() - path.get(1).getX(); + yDif = path.get(0).getY() - path.get(1).getY(); + distancePoints[0] = Math.sqrt((xDif * xDif) + (yDif * yDif)); + for(int i = 1; i < path.size(); i++){ + xDif = path.get(i).getX() - path.get(i - 1).getX(); + yDif = path.get(i).getY() - path.get(i - 1).getY(); + distancePoints[i] = distancePoints[i - 1] + Math.sqrt((xDif * xDif) + (yDif * yDif)); + } + return distancePoints; + } } From 88d500ccab2f4eefb87d501899c42eee0008ce46 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 13 Dec 2019 15:19:52 -0800 Subject: [PATCH 11/25] Update PurePursuit.java --- .../team2813/lib/purePursuit/PurePursuit.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 4151053..f8dcaf3 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -97,4 +97,21 @@ public double[] distancePoints(ArrayList path){ } return distancePoints; } + //finds the curvature of the path at a given point + public double findCurvature(Point2D P, Point2D Q, Point2D R){ + double x1 = P.getX() + 0.001; + double y1 = P.getY(); + double x2 = Q.getX(); + double y2 = Q.getY(); + double x3 = R.getX(); + double y3 = R.getY(); + double k1 = 0.5 * (((x1 * x1) + (y1 * y1)) - ((x2 * x2) + (y2 * y2)))/(x1 - x2); + double k2 = (y1 - y2)/(x1 - x2); + double b = 0.5 * ((x2 * x2) - (2 * x2 * k1) + (y2 * y2) - (x3 * x3) + (2 * x3 * k1) - (y3 * y3))/(x3 * k2 - y3 + + y2 - x2 * k2); + double a = k1 - k2 * b; + double r = Math.sqrt(((x1 - a) * (x1 - a)) + ((y1 - b) * (y1 -b))); + double curvature = 1 / r; + return curvature; + } } From d72f85de8269eb1cf6d3ae84e450f55481b65de2 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 13 Dec 2019 15:28:41 -0800 Subject: [PATCH 12/25] Update PurePursuit.java --- .../java/com/team2813/lib/purePursuit/PurePursuit.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index f8dcaf3..7b6e1b3 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -114,4 +114,13 @@ public double findCurvature(Point2D P, Point2D Q, Point2D R){ double curvature = 1 / r; return curvature; } + public double[] findPathCurvature(ArrayList path){ + double[] pathCurvature = new double[path.size()]; + pathCurvature[0] = 0.0; + for(int i = 1; i < path.size() - 1; i++){ + pathCurvature[i] = findCurvature(path.get(i), path.get(i - 1), path.get(i + 1)); + } + pathCurvature[path.size() - 1] = 0.0; + return pathCurvature; + } } From 9358aac8d336101d2f17b54936a1db25a354e9cc Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 13 Dec 2019 15:34:28 -0800 Subject: [PATCH 13/25] Update PurePursuit.java --- .../src/main/java/com/team2813/lib/purePursuit/PurePursuit.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 7b6e1b3..79b70c6 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -114,6 +114,7 @@ public double findCurvature(Point2D P, Point2D Q, Point2D R){ double curvature = 1 / r; return curvature; } + //returns an array with the curvature of the path at each point public double[] findPathCurvature(ArrayList path){ double[] pathCurvature = new double[path.size()]; pathCurvature[0] = 0.0; @@ -123,4 +124,5 @@ public double[] findPathCurvature(ArrayList path){ pathCurvature[path.size() - 1] = 0.0; return pathCurvature; } + } From c8bf61741344af0d1041130848654a6a3a671186 Mon Sep 17 00:00:00 2001 From: Gear Heads No Admin Date: Fri, 13 Dec 2019 15:59:57 -0800 Subject: [PATCH 14/25] removed empty method --- .../src/main/java/com/team2813/lib/purePursuit/PurePursuit.java | 1 + 1 file changed, 1 insertion(+) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 79b70c6..f03b925 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -125,4 +125,5 @@ public double[] findPathCurvature(ArrayList path){ return pathCurvature; } + } From 19e78cd5a51918a1f68d5079f0fa981b0d0e966e Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 13 Dec 2019 16:01:00 -0800 Subject: [PATCH 15/25] Update PurePursuit.java created method to return an array with target velocities for each point --- .../team2813/lib/purePursuit/PurePursuit.java | 25 ++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 79b70c6..e838973 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -124,5 +124,28 @@ public double[] findPathCurvature(ArrayList path){ pathCurvature[path.size() - 1] = 0.0; return pathCurvature; } - + //returns an array with target velocities for each point + public double[] pathVelocity(double[] pathCurvature, double maxVelocity, double minVelocity, double k){ + double min; + double[] pathVelocity = new double[pathCurvature.length]; + for(int i = 0; i < pathVelocity.length; i++){ + min = k / pathCurvature[i]; + try{ + min = k / pathCurvature[i]; + } + catch (ArithmeticException e){ + min = minVelocity; + } + if(maxVelocity > min){ + pathVelocity[i] = min; + } + else if(maxVelocity < min){ + pathVelocity[i] = maxVelocity; + } + else{ + pathVelocity[i] = min; + } + } + return pathVelocity; + } } From 16e73dff7a88c9579cb0f8002b8a4a772b282ff4 Mon Sep 17 00:00:00 2001 From: Gear Heads No Admin Date: Fri, 13 Dec 2019 18:43:05 -0800 Subject: [PATCH 16/25] Update PurePursuit.java --- .../team2813/lib/purePursuit/PurePursuit.java | 92 ++++++++++++------- 1 file changed, 61 insertions(+), 31 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index e838973..7f384d1 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -1,6 +1,8 @@ package com.team2813.lib.purePursuit; +import edu.wpi.first.wpilibj.Timer; + import java.awt.geom.Point2D; import java.util.ArrayList; @@ -21,11 +23,12 @@ public Point2D getLocation(double rightEncoderVal, double leftEncoderVal) { location.setLocation(xVal, yVal); return location; } + //injects points public ArrayList injectPoints(ArrayList path, double spacing) { int numSegments = path.size() - 1; - for(int i = 0; i < path.size(); i++){ - if(numSegments != 0){ + for (int i = 0; i < path.size(); i++) { + if (numSegments != 0) { Point2D startPosition = path.get(i); Point2D endPosition = path.get(i + 1); double xDif = endPosition.getX() - startPosition.getX(); @@ -33,11 +36,11 @@ public ArrayList injectPoints(ArrayList path, double spacing) Point2D vector = new Point2D.Double(); vector.setLocation(xDif, yDif); double vectorMagnitude = Math.sqrt((vector.getX() * vector.getX()) + (vector.getY() * vector.getY())); - double numPoints = Math.ceil(Math.sqrt(vectorMagnitude/spacing)); + double numPoints = Math.ceil(Math.sqrt(vectorMagnitude / spacing)); Point2D unitVector = new Point2D.Double(); - unitVector.setLocation(xDif/vectorMagnitude * spacing, yDif/vectorMagnitude * spacing); + unitVector.setLocation(xDif / vectorMagnitude * spacing, yDif / vectorMagnitude * spacing); Point2D newPoint = new Point2D.Double(); - for(int j = 0; j < numPoints; j++){ + for (int j = 0; j < numPoints; j++) { newPoint.setLocation(startPosition.getX() + unitVector.getX() * j, startPosition.getY() + unitVector.getY() * j); path.add(i + j + 1, newPoint); } @@ -47,29 +50,30 @@ public ArrayList injectPoints(ArrayList path, double spacing) } return path; } + //smooths path - public ArrayList smoother(ArrayList path, double a, double b, double tolerance){ + public ArrayList smoother(ArrayList path, double a, double b, double tolerance) { double change = tolerance; double newX = 0.0; double newY = 0.0; double x; double y; ArrayList newPath = new ArrayList(); - for(int i = 0; i < path.size(); i++){ + for (int i = 0; i < path.size(); i++) { newPath.add(path.get(i)); } - while(change >= tolerance){ + while (change >= tolerance) { change = 0.0; - for(int i = 1; i < path.size() - 1; i++){ - for(int j = 0; j < 2; j++){ - if(j == 0){ + for (int i = 1; i < path.size() - 1; i++) { + for (int j = 0; j < 2; j++) { + if (j == 0) { x = path.get(i).getX(); double aux = newPath.get(i).getX(); newX = newPath.get(i).getX(); newX += a * (x - aux) + b * (newPath.get(i - 1).getX() + newPath.get(i + 1).getX()) - (2.0 * aux); change += Math.abs(aux - newX); } - if(j == 1){ + if (j == 1) { y = path.get(i).getY(); double aux = newPath.get(i).getY(); newY = newPath.get(i).getY(); @@ -82,70 +86,96 @@ public ArrayList smoother(ArrayList path, double a, double b, } return newPath; } + //calculates the distance between points - public double[] distancePoints(ArrayList path){ + public double[] distancePoints(ArrayList path) { double xDif; double yDif; double[] distancePoints = new double[path.size()]; xDif = path.get(0).getX() - path.get(1).getX(); yDif = path.get(0).getY() - path.get(1).getY(); distancePoints[0] = Math.sqrt((xDif * xDif) + (yDif * yDif)); - for(int i = 1; i < path.size(); i++){ + for (int i = 1; i < path.size(); i++) { xDif = path.get(i).getX() - path.get(i - 1).getX(); yDif = path.get(i).getY() - path.get(i - 1).getY(); distancePoints[i] = distancePoints[i - 1] + Math.sqrt((xDif * xDif) + (yDif * yDif)); } return distancePoints; } + //finds the curvature of the path at a given point - public double findCurvature(Point2D P, Point2D Q, Point2D R){ + public double findCurvature(Point2D P, Point2D Q, Point2D R) { double x1 = P.getX() + 0.001; double y1 = P.getY(); double x2 = Q.getX(); double y2 = Q.getY(); double x3 = R.getX(); double y3 = R.getY(); - double k1 = 0.5 * (((x1 * x1) + (y1 * y1)) - ((x2 * x2) + (y2 * y2)))/(x1 - x2); - double k2 = (y1 - y2)/(x1 - x2); - double b = 0.5 * ((x2 * x2) - (2 * x2 * k1) + (y2 * y2) - (x3 * x3) + (2 * x3 * k1) - (y3 * y3))/(x3 * k2 - y3 + + double k1 = 0.5 * (((x1 * x1) + (y1 * y1)) - ((x2 * x2) + (y2 * y2))) / (x1 - x2); + double k2 = (y1 - y2) / (x1 - x2); + double b = 0.5 * ((x2 * x2) - (2 * x2 * k1) + (y2 * y2) - (x3 * x3) + (2 * x3 * k1) - (y3 * y3)) / (x3 * k2 - y3 + y2 - x2 * k2); double a = k1 - k2 * b; - double r = Math.sqrt(((x1 - a) * (x1 - a)) + ((y1 - b) * (y1 -b))); + double r = Math.sqrt(((x1 - a) * (x1 - a)) + ((y1 - b) * (y1 - b))); double curvature = 1 / r; return curvature; } + //returns an array with the curvature of the path at each point - public double[] findPathCurvature(ArrayList path){ + public double[] findPathCurvature(ArrayList path) { double[] pathCurvature = new double[path.size()]; pathCurvature[0] = 0.0; - for(int i = 1; i < path.size() - 1; i++){ + for (int i = 1; i < path.size() - 1; i++) { pathCurvature[i] = findCurvature(path.get(i), path.get(i - 1), path.get(i + 1)); } pathCurvature[path.size() - 1] = 0.0; return pathCurvature; } + //returns an array with target velocities for each point - public double[] pathVelocity(double[] pathCurvature, double maxVelocity, double minVelocity, double k){ + public double[] pathVelocity(double[] pathCurvature, double maxVelocity, double minVelocity, double k) { double min; double[] pathVelocity = new double[pathCurvature.length]; - for(int i = 0; i < pathVelocity.length; i++){ + for (int i = 0; i < pathVelocity.length; i++) { min = k / pathCurvature[i]; - try{ + try { min = k / pathCurvature[i]; - } - catch (ArithmeticException e){ + } catch (ArithmeticException e) { min = minVelocity; } - if(maxVelocity > min){ + if (maxVelocity > min) { pathVelocity[i] = min; - } - else if(maxVelocity < min){ + } else if (maxVelocity < min) { pathVelocity[i] = maxVelocity; - } - else{ + } else { pathVelocity[i] = min; } } return pathVelocity; } +//TODO:incomplete + //Part 3: calculates target robot path velocities for each point given the velocity setpoints above + public double[] targetVelocity(double[] pathVelocity, double maxAcceleration, ArrayList path) { + double max; + double[] distance = distancePoints(path); + double[] targetVelocity = new double[pathVelocity.length]; + for (int i = 1; i > targetVelocity.length; i++) { + //find target velocities + max = Math.sqrt(Math.pow(pathVelocity[i - 1], 2) + 2 * maxAcceleration * distance[i]); + targetVelocity[i] = max; + } + targetVelocity[targetVelocity.length - 1] = 0; + for (int i = targetVelocity.length - 1; i > 0; i --){ + } + return targetVelocity; + } + + //smooths the transition in velocity by limiting the rates + //TODO:incomplete + public double rateLimiter(double targetVelocity, double maxRateOfChange) { + double changeInTime = Timer.getFPGATimestamp(); //TODO: I'm not sure if this is accurate. Can someone look this over + double maxChange = changeInTime * maxRateOfChange; + return maxChange; // placeholder + } + } From 8aa650a64196526b554567b36ace27a07e5bf33a Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Sat, 14 Dec 2019 20:27:53 -0800 Subject: [PATCH 17/25] Update PurePursuit.java finished rate limiter --- .../team2813/lib/purePursuit/PurePursuit.java | 25 ++++++++++++++----- 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 7f384d1..d5fe5f4 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -9,6 +9,9 @@ import static java.awt.geom.Point2D.*; public class PurePursuit { + public double constrain(double value, double min, double max){ + return Math.min(Math.max(value, min), max); + } // finds coordinate location of the robot public Point2D getLocation(double rightEncoderVal, double leftEncoderVal) { double currentRightVal = 0.0; @@ -171,11 +174,21 @@ public double[] targetVelocity(double[] pathVelocity, double maxAcceleration, Ar } //smooths the transition in velocity by limiting the rates - //TODO:incomplete - public double rateLimiter(double targetVelocity, double maxRateOfChange) { - double changeInTime = Timer.getFPGATimestamp(); //TODO: I'm not sure if this is accurate. Can someone look this over - double maxChange = changeInTime * maxRateOfChange; - return maxChange; // placeholder + public double[] rateLimiter(double[] targetVelocity, double maxRateOfChange) { + double changeInTime = 0.0; + double maxChange = 0.0; + double[] output = new double[targetVelocity.length]; + changeInTime = Timer.getFPGATimestamp(); //TODO: I'm not sure if this is accurate. Can someone look this over + maxChange = changeInTime * maxRateOfChange; + output[0] = constrain(targetVelocity[0], -maxChange, maxChange); + targetVelocity[0] += output[0]; + for(int i = 1; i < targetVelocity.length; i++){ + changeInTime = Timer.getFPGATimestamp(); //TODO: I'm not sure if this is accurate. Can someone look this over + maxChange = changeInTime * maxRateOfChange; + output[i] = constrain(targetVelocity[i] - output[i - 1], -maxChange, maxChange); + targetVelocity[i] += output[i]; + } + return targetVelocity; } - + } From 8c6b728009b7ffaf55b7098f5d8b8e12f60ac455 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Sat, 14 Dec 2019 21:02:58 -0800 Subject: [PATCH 18/25] Update PurePursuit.java finished part 3 of velocities --- .../team2813/lib/purePursuit/PurePursuit.java | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index d5fe5f4..ea6c1bc 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -156,19 +156,26 @@ public double[] pathVelocity(double[] pathCurvature, double maxVelocity, double } return pathVelocity; } -//TODO:incomplete - //Part 3: calculates target robot path velocities for each point given the velocity setpoints above + //Part 3: calculates target robot path velocities for each point given the velocity setpoints above run through a rate limiter public double[] targetVelocity(double[] pathVelocity, double maxAcceleration, ArrayList path) { + double xDif; + double yDif; + double distancePoints; double max; double[] distance = distancePoints(path); double[] targetVelocity = new double[pathVelocity.length]; + double[] maxVelocity = new double[pathVelocity.length]; for (int i = 1; i > targetVelocity.length; i++) { - //find target velocities + //find maximum reachable velocities max = Math.sqrt(Math.pow(pathVelocity[i - 1], 2) + 2 * maxAcceleration * distance[i]); - targetVelocity[i] = max; + maxVelocity[i] = max; } targetVelocity[targetVelocity.length - 1] = 0; - for (int i = targetVelocity.length - 1; i > 0; i --){ + for (int i = targetVelocity.length - 2; i > 0; i --){ + xDif = path.get(i).getX() - path.get(i + 1).getX(); + yDif = path.get(i).getY() - path.get(i + 1).getY(); + distancePoints = Math.sqrt(Math.pow(xDif, 2) + Math.pow(yDif, 2)); + targetVelocity[i] = Math.min(pathVelocity[i], Math.sqrt(Math.pow(targetVelocity[i + 1], 2) + 2 * maxAcceleration * distancePoints)); } return targetVelocity; } @@ -178,17 +185,17 @@ public double[] rateLimiter(double[] targetVelocity, double maxRateOfChange) { double changeInTime = 0.0; double maxChange = 0.0; double[] output = new double[targetVelocity.length]; - changeInTime = Timer.getFPGATimestamp(); //TODO: I'm not sure if this is accurate. Can someone look this over + changeInTime = Timer.getFPGATimestamp(); maxChange = changeInTime * maxRateOfChange; output[0] = constrain(targetVelocity[0], -maxChange, maxChange); targetVelocity[0] += output[0]; for(int i = 1; i < targetVelocity.length; i++){ - changeInTime = Timer.getFPGATimestamp(); //TODO: I'm not sure if this is accurate. Can someone look this over + changeInTime = Timer.getFPGATimestamp(); maxChange = changeInTime * maxRateOfChange; output[i] = constrain(targetVelocity[i] - output[i - 1], -maxChange, maxChange); targetVelocity[i] += output[i]; } return targetVelocity; } - + } From 5a9f9896fdf2b631dfe23bdbf3b7bb270818e45f Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Sun, 15 Dec 2019 19:58:32 -0800 Subject: [PATCH 19/25] Update PurePursuit.java started to make method to find closest point the robot has not passed. also did some code cleanup --- .../team2813/lib/purePursuit/PurePursuit.java | 32 +++++++++++++++---- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index ea6c1bc..9affbf7 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -44,7 +44,8 @@ public ArrayList injectPoints(ArrayList path, double spacing) unitVector.setLocation(xDif / vectorMagnitude * spacing, yDif / vectorMagnitude * spacing); Point2D newPoint = new Point2D.Double(); for (int j = 0; j < numPoints; j++) { - newPoint.setLocation(startPosition.getX() + unitVector.getX() * j, startPosition.getY() + unitVector.getY() * j); + newPoint.setLocation(startPosition.getX() + unitVector.getX() * j, startPosition.getY() + + unitVector.getY() * j); path.add(i + j + 1, newPoint); } i += numPoints; @@ -73,14 +74,16 @@ public ArrayList smoother(ArrayList path, double a, double b, x = path.get(i).getX(); double aux = newPath.get(i).getX(); newX = newPath.get(i).getX(); - newX += a * (x - aux) + b * (newPath.get(i - 1).getX() + newPath.get(i + 1).getX()) - (2.0 * aux); + newX += a * (x - aux) + b * (newPath.get(i - 1).getX() + newPath.get(i + 1).getX()) - + (2.0 * aux); change += Math.abs(aux - newX); } if (j == 1) { y = path.get(i).getY(); double aux = newPath.get(i).getY(); newY = newPath.get(i).getY(); - newY += a * (y - aux) + b * (newPath.get(i - 1).getY() + newPath.get(i + 1).getY()) - (2.0 * aux); + newY += a * (y - aux) + b * (newPath.get(i - 1).getY() + newPath.get(i + 1).getY()) - + (2.0 * aux); change += Math.abs(aux - newY); } } @@ -116,8 +119,8 @@ public double findCurvature(Point2D P, Point2D Q, Point2D R) { double y3 = R.getY(); double k1 = 0.5 * (((x1 * x1) + (y1 * y1)) - ((x2 * x2) + (y2 * y2))) / (x1 - x2); double k2 = (y1 - y2) / (x1 - x2); - double b = 0.5 * ((x2 * x2) - (2 * x2 * k1) + (y2 * y2) - (x3 * x3) + (2 * x3 * k1) - (y3 * y3)) / (x3 * k2 - y3 + - y2 - x2 * k2); + double b = 0.5 * ((x2 * x2) - (2 * x2 * k1) + (y2 * y2) - (x3 * x3) + (2 * x3 * k1) - + (y3 * y3)) / (x3 * k2 - y3 + y2 - x2 * k2); double a = k1 - k2 * b; double r = Math.sqrt(((x1 - a) * (x1 - a)) + ((y1 - b) * (y1 - b))); double curvature = 1 / r; @@ -175,7 +178,8 @@ public double[] targetVelocity(double[] pathVelocity, double maxAcceleration, Ar xDif = path.get(i).getX() - path.get(i + 1).getX(); yDif = path.get(i).getY() - path.get(i + 1).getY(); distancePoints = Math.sqrt(Math.pow(xDif, 2) + Math.pow(yDif, 2)); - targetVelocity[i] = Math.min(pathVelocity[i], Math.sqrt(Math.pow(targetVelocity[i + 1], 2) + 2 * maxAcceleration * distancePoints)); + targetVelocity[i] = Math.min(pathVelocity[i], Math.sqrt(Math.pow(targetVelocity[i + 1], 2) + + 2 * maxAcceleration * distancePoints)); } return targetVelocity; } @@ -197,5 +201,19 @@ public double[] rateLimiter(double[] targetVelocity, double maxRateOfChange) { } return targetVelocity; } - + public Point2D closestPoint(ArrayList path, int indexLastPoint, double rightEncoderVal, + double leftEncoderVal){ + ArrayList searchPath = new ArrayList(); + for(int i = indexLastPoint + 1; i < path.size(); i++){ + searchPath.add(path.get(i)); + } + Point2D location = new Point2D.Double(); + location = getLocation(rightEncoderVal, leftEncoderVal); + int xDif; + int yDif; + double[] distance = new double[searchPath.size()]; + for(int i = 0; i < searchPath.size(); i++){ + + } + } } From 6bda902959b93531781f24667916e3807f4d0f34 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Sun, 15 Dec 2019 22:28:04 -0800 Subject: [PATCH 20/25] Update PurePursuit.java finished closest point method --- .../team2813/lib/purePursuit/PurePursuit.java | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 9affbf7..bcee395 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -203,17 +203,28 @@ public double[] rateLimiter(double[] targetVelocity, double maxRateOfChange) { } public Point2D closestPoint(ArrayList path, int indexLastPoint, double rightEncoderVal, double leftEncoderVal){ + Point2D closestPoint = new Point2D.Double(); ArrayList searchPath = new ArrayList(); for(int i = indexLastPoint + 1; i < path.size(); i++){ searchPath.add(path.get(i)); } Point2D location = new Point2D.Double(); location = getLocation(rightEncoderVal, leftEncoderVal); - int xDif; - int yDif; + double xDif; + double yDif; double[] distance = new double[searchPath.size()]; for(int i = 0; i < searchPath.size(); i++){ - + xDif = location.getX() - searchPath.get(i).getX(); + yDif = location.getY() - searchPath.get(i).getY(); + distance[i] = Math.sqrt(Math.pow(xDif, 2) + Math.pow(yDif, 2)); + } + int index = 0; + for(int i = 0; i < distance.length; i++){ + if(distance[index] > distance[i]){ + index = i; + } } + closestPoint = searchPath.get(index); + return closestPoint; } } From 6d7f7d654a67a334d22435e890e2f907c389c90d Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 20 Dec 2019 14:10:08 -0800 Subject: [PATCH 21/25] Update PurePursuit.java created method to get lookahead point --- .../team2813/lib/purePursuit/PurePursuit.java | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index bcee395..b8ad4b3 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -227,4 +227,35 @@ public Point2D closestPoint(ArrayList path, int indexLastPoint, double closestPoint = searchPath.get(index); return closestPoint; } + public Point2D lookAheadPoint(Point2D e, Point2D l, Point2D c, double r){ + double t = Timer.getFPGATimestamp(); + Point2D d = new Point2D.Double(); + Point2D f = new Point2D.Double(); + Point2D intersection = new Point2D.Double(); + d.setLocation(l.getX() - e.getX(), l.getY() - e.getY()); + f.setLocation(e.getX() - c.getX(), e.getY() - c.getY()); + double a = Math.pow(d.getX(), 2) + Math.pow(d.getY(), 2); + double b = 2 * ((f.getX() * d.getX()) + (f.getY() * d.getY())); + double dc = Math.pow(f.getX(), 2) + Math.pow(f.getY(), 2) - Math.pow(r, 2); + double discriminant = Math.pow(b, 2) - (4 * a * dc); + if(discriminant < 0){ + return intersection; + } + else{ + discriminant = Math.sqrt(discriminant); + double t1 = (-b - discriminant) / (2 * a); + double t2 = (-b + discriminant) / (2 * a); + if(t1 >= 0 && t1 <= 1){ + intersection.setLocation(e.getX() + t1 * d.getX(), e.getY() + t1 * d.getX()); + return intersection; + } + else if(t2 >= 0 && t2 <= 1){ + intersection.setLocation(e.getX() + t2 * d.getX(), e.getY() + t2 * d.getX()); + return intersection; + } + else{ + return intersection; + } + } + } } From 3996facec15c0ad50c89c4451dc7f1b52e489b58 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 20 Dec 2019 14:54:23 -0800 Subject: [PATCH 22/25] Update PurePursuit.java created method to find curvature of arc towards look ahead point --- .../team2813/lib/purePursuit/PurePursuit.java | 23 +++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index b8ad4b3..d561b65 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -227,6 +227,7 @@ public Point2D closestPoint(ArrayList path, int indexLastPoint, double closestPoint = searchPath.get(index); return closestPoint; } + //finds look ahead point public Point2D lookAheadPoint(Point2D e, Point2D l, Point2D c, double r){ double t = Timer.getFPGATimestamp(); Point2D d = new Point2D.Double(); @@ -243,8 +244,8 @@ public Point2D lookAheadPoint(Point2D e, Point2D l, Point2D c, double r){ } else{ discriminant = Math.sqrt(discriminant); - double t1 = (-b - discriminant) / (2 * a); - double t2 = (-b + discriminant) / (2 * a); + double t1 = (-b - discriminant)/(2 * a); + double t2 = (-b + discriminant)/(2 * a); if(t1 >= 0 && t1 <= 1){ intersection.setLocation(e.getX() + t1 * d.getX(), e.getY() + t1 * d.getX()); return intersection; @@ -258,4 +259,22 @@ else if(t2 >= 0 && t2 <= 1){ } } } + //finds curvature of arc to look ahead point + public double lookAheadCurvature(Point2D lookAheadPoint, double l, double r, double rightEncoderVal, + double leftEncoderVal){ + double curvature = (2 * lookAheadPoint.getX())/Math.pow(l, 2); + double robotAngle; + Point2D robotLocation = new Point2D.Double(); + robotLocation = getLocation(rightEncoderVal, leftEncoderVal); + robotAngle = Math.atan((lookAheadPoint.getY() - robotLocation.getY())/(lookAheadPoint.getX() - + robotLocation.getX())); + double a = -Math.tan(robotAngle); + double c = Math.tan(robotAngle) * robotLocation.getX() - robotLocation.getY(); + double d = Math.abs((a * lookAheadPoint.getX()) + lookAheadPoint.getY() + c)/ + Math.sqrt(Math.pow(a, 2) + 1); + double side = Math.signum(Math.sin(robotAngle) * (lookAheadPoint.getX() - robotLocation.getX()) - + Math.cos(robotAngle) * (lookAheadPoint.getY() - robotLocation.getY())); + curvature *= side; + return curvature; + } } From d501fc4ec6d1c519062a742da4ce70e97be4cbdf Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 20 Dec 2019 15:49:06 -0800 Subject: [PATCH 23/25] Update PurePursuit.java created method to find wheel velocities --- .../team2813/lib/purePursuit/PurePursuit.java | 25 ++++++++++++++++--- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index d561b65..b00053e 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -201,9 +201,9 @@ public double[] rateLimiter(double[] targetVelocity, double maxRateOfChange) { } return targetVelocity; } - public Point2D closestPoint(ArrayList path, int indexLastPoint, double rightEncoderVal, + //returns index of the closest point to the robot + public int closestPoint(ArrayList path, int indexLastPoint, double rightEncoderVal, double leftEncoderVal){ - Point2D closestPoint = new Point2D.Double(); ArrayList searchPath = new ArrayList(); for(int i = indexLastPoint + 1; i < path.size(); i++){ searchPath.add(path.get(i)); @@ -224,8 +224,7 @@ public Point2D closestPoint(ArrayList path, int indexLastPoint, double index = i; } } - closestPoint = searchPath.get(index); - return closestPoint; + return index; } //finds look ahead point public Point2D lookAheadPoint(Point2D e, Point2D l, Point2D c, double r){ @@ -277,4 +276,22 @@ public double lookAheadCurvature(Point2D lookAheadPoint, double l, double r, dou curvature *= side; return curvature; } + //finds wheel velocities + public double[] wheelVelocities(double c, ArrayList path, int closestPointIndex, double t, + double maxVelocity, double minVelocity, double k, double maxRateOfChange, + double maxAcceleration){ + double targetVelocities[] = new double[path.size()]; + targetVelocities = pathVelocity(findPathCurvature(path), maxVelocity, minVelocity, k); + targetVelocities = rateLimiter(targetVelocities, maxRateOfChange); + targetVelocities = targetVelocity(targetVelocities, maxAcceleration, path); + double targetVelocity = targetVelocities[closestPointIndex]; + double r = 1/c; + double[] wheelVelocity = new double[2]; + double l = targetVelocity * (2 + (c * t))/2; + double wvr = targetVelocity * (2 - (c * t))/2; + wheelVelocity[0] = l; + wheelVelocity[1] = wvr; + return wheelVelocity; + } + } From 582614cc633aec9006c1b1618abb3ea4787362ee Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Fri, 20 Dec 2019 16:06:51 -0800 Subject: [PATCH 24/25] Update PurePursuit.java created method to find power for wheels values. more work needed --- .../main/java/com/team2813/lib/purePursuit/PurePursuit.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index b00053e..0c0cd3e 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -293,5 +293,9 @@ public double[] wheelVelocities(double c, ArrayList path, int closestPo wheelVelocity[1] = wvr; return wheelVelocity; } - + //finds wheel power for the left/right wheels + public double[] wheelPower(double[] wheelVelocities, double kv, double ka, double kp){ + double ff; + double fb; + } } From 52090459a7900835fbd8f38ee006d6c7f0a90766 Mon Sep 17 00:00:00 2001 From: abhineetpal Date: Sat, 28 Dec 2019 11:47:48 -0800 Subject: [PATCH 25/25] Update PurePursuit.java --- .../src/main/java/com/team2813/lib/purePursuit/PurePursuit.java | 1 - 1 file changed, 1 deletion(-) diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java index 0c0cd3e..f1475ee 100644 --- a/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/purePursuit/PurePursuit.java @@ -285,7 +285,6 @@ public double[] wheelVelocities(double c, ArrayList path, int closestPo targetVelocities = rateLimiter(targetVelocities, maxRateOfChange); targetVelocities = targetVelocity(targetVelocities, maxAcceleration, path); double targetVelocity = targetVelocities[closestPointIndex]; - double r = 1/c; double[] wheelVelocity = new double[2]; double l = targetVelocity * (2 + (c * t))/2; double wvr = targetVelocity * (2 - (c * t))/2;