Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

src/main/deploy/pathplanner/Move Forward.path

@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":null,"nextControl":{"x":2.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":{"x":1.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
+{"waypoints":[{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":null,"nextControl":{"x":4.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":{"x":3.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}

src/main/java/frc4388/robot/RobotContainer.java

@@ -341,6 +341,52 @@ public class RobotContainer {
         .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
   }
 
+  /**
+   * Generate autonomous
+   * @param maxVel max velocity for the path (null to override default value of 5.0)
+   * @param maxAccel max acceleration for the path (null to override default value of 5.0)
+   * @param inputs strings (paths) or commands you want to run (in order)
+   * @return array of commands
+   */
+  public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
+    maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY);
+    maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
+
+    ArrayList<Command> commands = new ArrayList<Command>();
+
+    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
+    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
+    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
+    thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+    // parse input
+    for (int i=0; i<inputs.length; i++) {
+      if (inputs[i] instanceof String) {
+
+        PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
+        PathPlannerState initState = (PathPlannerState) traj.sample(0);
+        commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
+        commands.add(new PPSwerveControllerCommand(
+                          traj,
+                          m_robotSwerveDrive::getOdometry,
+                          m_robotSwerveDrive.m_kinematics,
+                          xController,
+                          yController,
+                          thetaController,
+                          m_robotSwerveDrive::setModuleStates,
+                          m_robotSwerveDrive));
+      }
+      if (inputs[i] instanceof Command) {
+        commands.add((Command) inputs[i]);
+      }
+    }
+    
+    commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
+    Command[] ret = new Command[commands.size()];
+    ret = commands.toArray(ret);
+    return ret;
+  }
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
@@ -367,7 +413,17 @@ public class RobotContainer {
     //   return new RunCommand(() -> {
     //   }).withName("No Autonomous Path");
     // }
-    return null;
+
+    // ! this will run each of the specified PathPlanner paths in sequence.
+    // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
+
+    // ! this will run each of the specified PathPlanner paths in sequence, while simultaneously running the intake throughout all the paths.
+    // * return new ParallelCommandGroup(buildAuto(null,
+    // *                                           null,
+    // *                                           new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
+    // *                                           new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
+
+    return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Move Forward")); // test command
   }
 
   public static XboxController getDriverController() {

src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java

@@ -5,6 +5,9 @@
 package frc4388.robot.commands.ShooterCommands;
 
 import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.stream.Collector;
+import java.util.stream.Collectors;
 
 import org.opencv.core.Point;
 
@@ -80,17 +83,9 @@ public class TrackTarget extends CommandBase {
       m_visionOdometry.setLEDs(true);
       points = m_visionOdometry.getTargetPoints();
       
+      points = filterPoints(points);
+      
       Point average = VisionOdometry.averagePoint(points);
-      
-      for(Point point : points) {
-        Vector2D difference = new Vector2D(point);
-        difference.subtract(new Vector2D(average));
-
-        if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD)
-        points.remove(point);
-      }
-      
-      average = VisionOdometry.averagePoint(points);
       DesmosServer.putPoint("average", average);
       
       for(int i = 0; i < points.size(); i++) {
@@ -151,6 +146,26 @@ public class TrackTarget extends CommandBase {
     // m_turret.runshooterRotatePID(m_targetAngle);
   }
 
+  public ArrayList<Point> filterPoints(ArrayList<Point> points) {
+    Point average = VisionOdometry.averagePoint(points);
+      
+    HashMap<Point, Double> pointDistances = new HashMap<>();
+    double distanceSum = 0;
+
+    for(Point point : points) {
+      Vector2D difference = new Vector2D(point);
+      difference.subtract(new Vector2D(average));
+
+      double mag = difference.magnitude();
+      distanceSum += mag;
+
+      pointDistances.put(point, mag);
+    }
+
+    final double averageDist = distanceSum / points.size();
+    return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
+  }
+
   public final double distanceRegression(double distance) {
     return (79.6078 * Math.pow(1.01343, distance) - 56.6671);
   }