FF-19 Basic Pure Pursuit and tuned odometry for new drivetrain

.gitignore

@@ -1,3 +1,5 @@
+**/.DS_STORE
+
 # Built application files
 *.apk
 *.aar

TeamCode/build.gradle

@@ -33,8 +33,7 @@ dependencies {
     implementation project(':FtcRobotController')
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
     // https://mvnrepository.com/artifact/org.opencv/opencv-android
-    implementation('org.opencv:opencv-android:1.0.1')
-    implementation 'de.esoco:coroutines:0.9.1'
+    implementation('de.esoco:coroutines:0.9.1')
     implementation('com.google.ar:core:1.26.0')
     implementation ('org.openftc:easyopencv:1.4.4')
 }

TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/Robot.java

@@ -1,5 +1,7 @@
 package com.kuriosityrobotics.firstforward.robot;
 
+import com.kuriosityrobotics.firstforward.robot.math.Point;
+import com.kuriosityrobotics.firstforward.robot.modules.Drivetrain;
 import com.kuriosityrobotics.firstforward.robot.modules.DrivetrainModule;
 import com.kuriosityrobotics.firstforward.robot.modules.Module;
 import com.kuriosityrobotics.firstforward.robot.modules.ModuleThread;
@@ -19,9 +21,10 @@ public class Robot {
     private static final String configLocation = "configurations/mainconfig.toml";
 
     private final Thread[] threads;
-    private final Module[] modules;
+    public final SensorThread sensorThread;
 
-    public final DrivetrainModule drivetrainModule;
+    private final Module[] modules;
+    public final Drivetrain drivetrain;
     public final TelemetryDump telemetryDump;
 
     public final HardwareMap hardwareMap;
@@ -45,18 +48,22 @@ public Robot(HardwareMap hardwareMap, Telemetry telemetry, LinearOpMode linearOp
             throw new NotFoundException("One or more of the REV hubs could not be found. More info: " + e);
         }
 
-        drivetrainModule = new DrivetrainModule(this);
+        drivetrain = new Drivetrain(this);
 
         modules = new Module[]{
-                drivetrainModule
+                drivetrain
         };
+
+        sensorThread = new SensorThread(this, configLocation);
         threads = new Thread[]{
-                new Thread(new SensorThread(this, configLocation)),
+                new Thread(sensorThread),
                 new Thread(new ModuleThread(this, this.modules))
         };
+
+        start();
     }
 
-    public void start() {
+    private void start() {
         for (Thread thread : threads) {
             thread.start();
         }
@@ -77,4 +84,8 @@ public boolean isOpModeActive() {
     public boolean running() {
         return (!linearOpMode.isStopRequested() && !linearOpMode.isStarted()) || isOpModeActive();
     }
+
+    public boolean started() {
+        return linearOpMode.isStarted();
+    }
 }

TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/math/Circle.java

@@ -1,5 +1,7 @@
 package com.kuriosityrobotics.firstforward.robot.math;
 
+import android.util.Log;
+
 import java.util.ArrayList;
 
 public class Circle {
@@ -11,17 +13,14 @@ public Circle(Point center, double radius) {
         this.radius = radius;
     }
 
-    public Circle() {
-    }
-
     public double centerDistance(Line line) {
         double distance;
         if (line.isVertical()) {
             distance = Math.abs(center.x - line.startPoint.x);
-        }else if (line.slope == 0) {
+        }else if (line.getSlope() == 0) {
             distance = Math.abs(center.y - line.startPoint.y);
         }else {
-            Line perpendicular = new Line(center, -1 / line.slope);
+            Line perpendicular = new Line(center, -1 / line.getSlope());
             Point intersection = line.getIntersection(perpendicular);
             distance = center.distance(intersection);
         }
@@ -45,27 +44,32 @@ public ArrayList<Point> getIntersections(Line line) {
             if (sqrtExpression != 0) {
                 intersections.add(new Point(line.startPoint.x, center.y + sqrtExpression));
             }
-        } else if (line.slope == 0) { // line equation is y = startPoint.y
+        } else if (line.getSlope() == 0) { // line equation is y = startPoint.y
             double sqrtExpression = Math.sqrt(radius*radius - Math.pow(line.startPoint.y - center.y, 2));
 
             intersections.add(new Point(center.x + sqrtExpression, line.startPoint.y));
             if (sqrtExpression != 0) {
                 intersections.add(new Point(center.x - sqrtExpression, line.startPoint.y));
             }
         } else { // line equation is y = slope*x + yInt
-            double a = line.slope*line.slope + 1;
-            double b = 2 * line.yInt * line.slope - 2 * center.y * line.slope - 2 * center.x;
-            double c = Math.pow(line.yInt - center.y, 2) + center.x*center.x - radius*radius;
+            double a = line.getSlope() * line.getSlope() + 1;
+            double b = 2 * line.getYInt() * line.getSlope() - 2 * center.y * line.getSlope() - 2 * center.x;
+            double c = Math.pow(line.getYInt() - center.y, 2) + center.x*center.x - radius*radius;
 
             double[] xValues = quadraticFormula(a, b, c);
-            intersections.add(new Point(xValues[0], xValues[0]*line.slope + line.yInt));
+            intersections.add(new Point(xValues[0], xValues[0]* line.getSlope() + line.getYInt()));
             if (xValues[0] != xValues[1]) {
-                intersections.add(new Point(xValues[1], xValues[1]*line.slope + line.yInt));
+                intersections.add(new Point(xValues[1], xValues[1]* line.getSlope() + line.getYInt()));
             }
         }
         return intersections;
     }
 
+    public ArrayList<Point> getSegmentIntersections(Line line){
+        ArrayList<Point> intersections = getIntersections(line);
+        return line.pointsOnLine(intersections);
+    }
+
     public static double[] quadraticFormula(double a, double b, double c) {
         double discriminant = b * b - 4 * a * c;
 

TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/math/Line.java

@@ -1,66 +1,52 @@
 package com.kuriosityrobotics.firstforward.robot.math;
 
+import static com.kuriosityrobotics.firstforward.robot.math.MathUtil.doublesEqual;
+
 import java.util.ArrayList;
 
 public class Line {
     public Point startPoint;
-    public Point endPoint = null;
-    public double slope;
-    public double yInt;
+    public Point endPoint;
 
     //for pathfollow (LINE SEGMENT)
-    public Line(Point startPoint, Point endPoint){
+    public Line(Point startPoint, Point endPoint) {
         this.startPoint = startPoint;
         this.endPoint = endPoint;
-        if (Math.abs(endPoint.x - startPoint.x) < .0001) {
-            //it needs to be init-ed
-            slope = 9999999;
-        }
-        slope = (endPoint.y - startPoint.y)/(endPoint.x - startPoint.x);
-        yInt = startPoint.y - slope*startPoint.x;
     }
+
     //for finding intersections (LINE)
-    public Line(Point point, double slope){
-        this.startPoint = point;
-        this.slope = slope;
-        yInt = point.y - slope*point.x;
-    }
-    //null line
-    public Line(){
+    public Line(Point point, double slope) {
+        this(point, new Point(point.x + 1, point.y + slope));
     }
 
     public boolean isVertical() {
-        boolean vertical;
-        if (endPoint == null) {
-            vertical = false;
-        }else {
-            vertical = Math.abs(endPoint.x - startPoint.x) < .0001;
-        }
-        return vertical;
+        return doublesEqual(startPoint.x, endPoint.x);
     }
 
-    public Point getIntersection(Line other){
-        if (slope == other.slope){ return null; }
+    public Point getIntersection(Line other) {
+        if (getSlope() == other.getSlope()) {
+            return null;
+        }
         if (this.isVertical() && !other.isVertical()) {
-            return new Point(startPoint.x, startPoint.x*other.slope + other.yInt);
-        }else if (!this.isVertical() && other.isVertical()) {
-            return new Point(other.startPoint.x, other.startPoint.x*slope + yInt);
+            return new Point(startPoint.x, startPoint.x * other.getSlope() + other.getYInt());
+        } else if (!this.isVertical() && other.isVertical()) {
+            return new Point(other.startPoint.x, other.startPoint.x * getSlope() + getYInt());
         }
         //y = slope(x - startPoint.x) + startPoint.y
         //y = slope*x + (startPoint.y - slope*startPoint.x)
-        double a = slope;
-        double b = startPoint.y - slope*startPoint.x;
-        double c = other.slope;
-        double d = other.yInt;
+        double a = getSlope();
+        double b = startPoint.y - getSlope() * startPoint.x;
+        double c = other.getSlope();
+        double d = other.getYInt();
 
-        double x = (d-b)/(a-c);
-        double y = slope*x + yInt;
+        double x = (d - b) / (a - c);
+        double y = getSlope() * x + getYInt();
 
         return new Point(x, y);
     }
 
-    public Point closerToEnd(ArrayList<Point> points){
-        double minDistance = 999999999;
+    public Point closerToEnd(ArrayList<Point> points) {
+        double minDistance = Double.MAX_VALUE;
         Point closest = points.get(0);
         for (Point p : points) {
             if (p.distance(endPoint) < minDistance) {
@@ -71,21 +57,46 @@ public Point closerToEnd(ArrayList<Point> points){
         return closest;
     }
 
-    public ArrayList<Point> pointsInRange(ArrayList<Point> points, double xMin, double xMax){
-        ArrayList<Point> inRange = new ArrayList<Point>();
+    public ArrayList<Point> pointsOnLine(ArrayList<Point> points) {
+        ArrayList<Point> onLine = new ArrayList<>();
 
-        if (isVertical() && xMin <= startPoint.x && startPoint.x <= xMax){
-            return points;
-        }
-        for (Point p : points) {
-            if (p.isOnSegment(new Line(new Point(xMin, slope*xMin + yInt), new Point(xMax, slope*xMax + yInt)))) {
-                inRange.add(p);
+        double minX = Math.min(startPoint.x, endPoint.x);
+        double maxX = Math.max(startPoint.x, endPoint.x);
+        double minY = Math.min(startPoint.y, endPoint.y);
+        double maxY = Math.max(startPoint.y, endPoint.y);
+
+        for (Point point : points) {
+            boolean withinX = point.x >= minX && point.x <= maxX;
+            boolean withinY = point.y >= minY && point.y <= maxY;
+
+            if (withinX && withinY) {
+                onLine.add(point);
             }
         }
-        return inRange;
+
+        return onLine;
+    }
+
+    public boolean containsPoint(Point point) {
+        double minX = Math.min(startPoint.x, endPoint.x);
+        double maxX = Math.max(startPoint.x, endPoint.x);
+        double minY = Math.min(startPoint.y, endPoint.y);
+        double maxY = Math.max(startPoint.y, endPoint.y);
+
+        boolean withinX = point.x >= minX && point.x <= maxX;
+        boolean withinY = point.y >= minY && point.y <= maxY;
+
+        return withinX && withinY;
+    }
+
+    public double getSlope() {
+        if (isVertical()) {
+            return Double.MAX_VALUE; // TODO: not use weird nudging like this, make actual cases
+        }
+        return (endPoint.y - startPoint.y) / (endPoint.x - startPoint.x);
     }
 
-    public double segmentDistance(Point p, double xMin, double xMax) {
-        return 0;
+    public double getYInt() {
+        return startPoint.y - getSlope() * startPoint.x;
     }
 }

TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/math/MathUtil.java

@@ -0,0 +1,45 @@
+package com.kuriosityrobotics.firstforward.robot.math;
+
+public class MathUtil {
+    private static final double EPSILON = 0.00001;
+
+    /**
+     * Wraps angle (in radians) to a value from -pi to pi
+     *
+     * @param angle Angle to be wrapped
+     * @return The wrapped angle, between -pi and pi
+     */
+    public static double angleWrap(double angle) {
+        return angleWrap(angle, 0);
+    }
+
+    /**
+     * Wraps an angle (in radians) to a value within pi of centerOfWrap.
+     *
+     * @param angle        The angle to be wrapped
+     * @param centerOfWrap The center of the boundary in which the angle can be wrapped to.
+     * @return The wrapped angle, which will lie within pi of the centerOfWrap.
+     */
+    public static double angleWrap(double angle, double centerOfWrap) {
+        if (angle > centerOfWrap + Math.PI) {
+            return angleWrap(angle - (2 * Math.PI), centerOfWrap);
+        } else if (angle < centerOfWrap - Math.PI) {
+            return angleWrap(angle + (2 * Math.PI), centerOfWrap);
+        } else {
+            return angle;
+        }
+    }
+
+    public static boolean doublesEqual(double a, double b) {
+        return Math.abs(a - b) < EPSILON;
+    }
+
+    public static double max(double... in) {
+        double max = Double.MIN_VALUE;
+        for (double x : in) {
+            max = Math.max(max, x);
+        }
+
+        return max;
+    }
+}