Merge pull request #81 from rh-robotics/rayne-pathfinding-framework

pathfinding framework

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/RobotMethods.java

@@ -5,28 +5,44 @@
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware;
 import org.firstinspires.ftc.teamcode.subsystems.stateMachineController.Context;
+import org.firstinspires.ftc.teamcode.subsystems.stateMachineController.Obstacle;
+
+import java.util.ArrayList;
+import java.util.HashMap;
 
 public class RobotMethods {
     static float wheelEncoderPPR = 537.7f; // PPR
     static int wheelDiameter = 96; // mm
     static double mmPerEncoderTick = (360/wheelEncoderPPR)/360*(wheelDiameter*Math.PI); // 0.56089435511 mm
     static double distanceBetweenWheels = 264; // mm
 
-    // TODO: Pathfinding here.
     public static void driveTo(Hardware robot, Context context, double targetX, double targetY,
                                double motorRunPower, double tolerance, Telemetry telemetry) {
+        ArrayList<double[]> path = findPath(context, targetX, targetY);
+        for (double[] point : path) {
+            goToPosition(robot, context, point[0], point[1], motorRunPower, tolerance, telemetry);
+        }
+    }
+
+    // TODO: Set up actual pathfinding. (Or at least something that can handle general obstacles
+    //  (eg. backboard).
+    private static ArrayList<double[]> findPath(Context context, double targetX, double targetY) {
+        ArrayList<double[]> path = new ArrayList<>();
+
         double[] passingLocation = new double[] {300,200}; // Where we pass through "middle".
 
         // If we're passing through the "middle" of the field.
         if (context.getY() < 250 && targetY > 250) {
-            goToPosition(robot, context, 300, 200, motorRunPower, tolerance, telemetry);
-            goToPosition(robot, context, 300, 350, motorRunPower, tolerance, telemetry);
+            path.add(new double[] {300, 200});
+            path.add(new double[] {300, 350});
         } else if (context.getY() > 250 && targetY < 250) {
-            goToPosition(robot, context, 300, 350, motorRunPower, tolerance, telemetry);
-            goToPosition(robot, context, 300, 200, motorRunPower, tolerance, telemetry);
+            path.add(new double[] {300, 350});
+            path.add(new double[] {300, 200});
         }
 
-        goToPosition(robot, context, targetX, targetY, motorRunPower, tolerance, telemetry);
+        path.add(new double[] {targetX, targetY});
+
+        return path;
     }
 
     public static void goToPosition(Hardware robot, Context context, double targetX, double targetY,

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/stateMachineController/Context.java

@@ -8,6 +8,7 @@ public class Context {
     boolean inIntakePosition = false;
     byte pixelsInControl = 0;
     double[] location = new double[] {-1.0, -1.0, 0};
+    Obstacle[] obstacles = new Obstacle[] {};
 
     public void setLocation(double x, double y, double direction) {
         this.location[0] = x;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/stateMachineController/Obstacle.java

@@ -0,0 +1,13 @@
+package org.firstinspires.ftc.teamcode.subsystems.stateMachineController;
+
+public class Obstacle {
+    // Assuming for now that all obstacles are rectangular.
+    double[] walls;
+    double[] dialatedWalls;
+    final double MAX_ROBOT_RADIUS = 547.32; // mm
+    public Obstacle(double leftX, double rightX, double topY, double bottomY) {
+        walls = new double[] {leftX, rightX, topY, bottomY};
+        dialatedWalls = new double[] {leftX + MAX_ROBOT_RADIUS, rightX + MAX_ROBOT_RADIUS,
+                topY + MAX_ROBOT_RADIUS, bottomY + MAX_ROBOT_RADIUS};
+    }
+}