diff --git a/src/main/deploy/pathplanner/Move Forward.path b/src/main/deploy/pathplanner/Move Forward.path index ff9f904..653ed78 100644 --- a/src/main/deploy/pathplanner/Move Forward.path +++ b/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} \ No newline at end of file +{"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} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cf3b893..9ceded1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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 commands = new ArrayList(); + + 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 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() { diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index f489de4..79f2a11 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/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 filterPoints(ArrayList points) { + Point average = VisionOdometry.averagePoint(points); + + HashMap 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) 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); }