From 9a0e3f66d2b5be48624a0472f3973c943264c5a8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 10 Mar 2022 17:44:27 -0700 Subject: [PATCH] changed/fixed some things up --- src/main/java/frc4388/robot/Constants.java | 4 +- src/main/java/frc4388/robot/Robot.java | 10 +- .../java/frc4388/robot/RobotContainer.java | 502 +++++++++--------- 3 files changed, 259 insertions(+), 257 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 64f285c..bef637c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -149,8 +149,8 @@ public final class Constants { public static final class ClimberConstants { /* TODO: Update motor IDS */ public static final int SHOULDER_ID = 1; - public static final int ELBOW_ID = 3; - public static final int GYRO_ID = 14; + public static final int ELBOW_ID = 30; + public static final int GYRO_ID = 31; // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1e1ae82..b83b7c8 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -131,9 +131,9 @@ public class Robot extends TimedRobot { // print odometry data to smart dashboard for debugging (if causing timeout // errors, you can comment it) - SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); - SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); - SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); + // SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); + // SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); + // SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); } /** @@ -150,7 +150,7 @@ public class Robot extends TimedRobot { File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner") .resolve("recording." + System.currentTimeMillis() + ".path").toFile(); if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { - m_robotContainer.createPath(null, null, false).write(outputFile); + // m_robotContainer.createPath(null, null, false).write(outputFile); LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath()); } else LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); @@ -187,7 +187,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { LOGGER.fine("teleopInit()"); - m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); + // m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a48646f..6290768 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -96,17 +96,17 @@ public class RobotContainer { private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, - m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); + // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, + // m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); - private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); - private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); - private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); - //private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); - private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); - private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); + // private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); + // private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); + // private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); + // //private final LED m_robotLED = new LED(m_robotMap.LEDController); + // private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + // private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); + // private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + // private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); /* Controllers */ @@ -115,7 +115,7 @@ public class RobotContainer { /* Autonomous */ private PathPlannerTrajectory loadedPathTrajectory = null; - private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); + // private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); private final List pathPoints = new ArrayList<>(); private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording"); @@ -148,39 +148,39 @@ public class RobotContainer { // Turret default command //m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); - m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)); + // m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)); - //Swerve Drive - m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput( - getDriverController().getLeftX(), - getDriverController().getLeftY(), - getDriverController().getRightX(), - getDriverController().getRightY(), - true), - m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); - //Intake with Triggers - m_robotIntake.setDefaultCommand( - new RunCommand(() -> m_robotIntake.runWithTriggers( - getOperatorController().getLeftTriggerAxis(), - getOperatorController().getRightTriggerAxis()), - m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - //Storage Management - m_robotStorage.setDefaultCommand( - new RunCommand(() -> m_robotStorage.manageStorage(), - m_robotStorage).withName("Storage manageStorage defaultCommand")); - //Serializer Management - m_robotSerializer.setDefaultCommand( - new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(), - m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); + // //Swerve Drive + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput( + // getDriverController().getLeftX(), + // getDriverController().getLeftY(), + // getDriverController().getRightX(), + // getDriverController().getRightY(), + // true), + // m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + // //Intake with Triggers + // m_robotIntake.setDefaultCommand( + // new RunCommand(() -> m_robotIntake.runWithTriggers( + // getOperatorController().getLeftTriggerAxis(), + // getOperatorController().getRightTriggerAxis()), + // m_robotIntake).withName("Intake runWithTriggers defaultCommand")); + // //Storage Management + // m_robotStorage.setDefaultCommand( + // new RunCommand(() -> m_robotStorage.manageStorage(), + // m_robotStorage).withName("Storage manageStorage defaultCommand")); + // //Serializer Management + // m_robotSerializer.setDefaultCommand( + // new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(), + // m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // continually sends updates to the Blinkin LED controller to keep the lights on // m_robotLED // .setDefaultCommand(new RunCommand(m_robotLED::updateLED, // m_robotLED).withName("LED update defaultCommand")); - autoInit(); - recordInit(); + // autoInit(); + // recordInit(); } /** @@ -193,27 +193,27 @@ public class RobotContainer { /* Driver Buttons */ // "XboxController.Button.kBack" was undefined yet, 7 works just fine - new JoystickButton(getDriverController(), XboxController.Button.kBack.value) - .whenPressed(m_robotSwerveDrive::resetGyro); + // new JoystickButton(getDriverController(), XboxController.Button.kBack.value) + // .whenPressed(m_robotSwerveDrive::resetGyro); - new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) - // new XboxControllerRawButton(m_driverXbox, - // XboxControllerRaw.LEFT_BUMPER_BUTTON) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + // new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) + // // new XboxControllerRawButton(m_driverXbox, + // // XboxControllerRaw.LEFT_BUMPER_BUTTON) + // .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); - new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) - // new XboxControllerRawButton(m_driverXbox, - // XboxControllerRaw.RIGHT_BUMPER_BUTTON) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + // new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) + // // new XboxControllerRawButton(m_driverXbox, + // // XboxControllerRaw.RIGHT_BUMPER_BUTTON) + // .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - new JoystickButton(getDriverController(), XboxController.Button.kA.value) - .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); + // new JoystickButton(getDriverController(), XboxController.Button.kA.value) + // .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp - .whenPressed(() -> m_robotMap.leftFront.reset()) - .whenPressed(() -> m_robotMap.rightFront.reset()) - .whenPressed(() -> m_robotMap.leftBack.reset()) - .whenPressed(() -> m_robotMap.rightBack.reset()); + // new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp + // .whenPressed(() -> m_robotMap.leftFront.reset()) + // .whenPressed(() -> m_robotMap.rightFront.reset()) + // .whenPressed(() -> m_robotMap.leftBack.reset()) + // .whenPressed(() -> m_robotMap.rightBack.reset()); /* Operator Buttons */ // run claws @@ -232,6 +232,7 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2))) .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); + } /* * // activates "BoomBoom" @@ -241,31 +242,31 @@ public class RobotContainer { */ //Extender - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(() -> m_robotIntake.runExtender(true)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whenPressed(() -> m_robotIntake.runExtender(true)); - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(() -> m_robotIntake.runExtender(false)); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(() -> m_robotIntake.runExtender(false)); - //Storage - new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)) - .whenReleased(() -> m_robotStorage.runStorage(0.0)); + // //Storage + // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + // .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)) + // .whenReleased(() -> m_robotStorage.runStorage(0.0)); - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)) - .whenReleased(() -> m_robotStorage.runStorage(0.0)); + // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + // .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)) + // .whenReleased(() -> m_robotStorage.runStorage(0.0)); - //Shooter - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); + // //Shooter + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); - } + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); + // } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -273,26 +274,27 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - if (loadedPathTrajectory != null) { - PIDController xController = SwerveDriveConstants.X_CONTROLLER; - PIDController yController = SwerveDriveConstants.Y_CONTROLLER; - ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; - thetaController.enableContinuousInput(-Math.PI, Math.PI); + // if (loadedPathTrajectory != null) { + // PIDController xController = SwerveDriveConstants.X_CONTROLLER; + // PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + // ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + // thetaController.enableContinuousInput(-Math.PI, Math.PI); - PathPlannerState initialState = loadedPathTrajectory.getInitialState(); - Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); - return new SequentialCommandGroup( - new InstantCommand(m_robotSwerveDrive.m_gyro::reset), - new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)), - new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry, - m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, - m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), - new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path"); - } else { - LOGGER.severe("No auto selected."); - return new RunCommand(() -> { - }).withName("No Autonomous Path"); - } + // PathPlannerState initialState = loadedPathTrajectory.getInitialState(); + // Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); + // return new SequentialCommandGroup( + // new InstantCommand(m_robotSwerveDrive.m_gyro::reset), + // new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)), + // new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry, + // m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, + // m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), + // new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path"); + // } else { + // LOGGER.severe("No auto selected."); + // return new RunCommand(() -> { + // }).withName("No Autonomous Path"); + // } + return null; } public XboxController getDriverController() { @@ -304,18 +306,18 @@ public class RobotContainer { * * @return Odometry */ - public Pose2d getOdometry() { - return m_robotSwerveDrive.getOdometry(); - } + // public Pose2d getOdometry() { + // return m_robotSwerveDrive.getOdometry(); + // } /** * Set odometry to given pose. * * @param pose Pose to set odometry to. */ - public void resetOdometry(Pose2d pose) { - m_robotSwerveDrive.resetOdometry(pose); - } + // public void resetOdometry(Pose2d pose) { + // m_robotSwerveDrive.resetOdometry(pose); + // } public XboxController getOperatorController() { return m_operatorXbox; @@ -328,169 +330,169 @@ public class RobotContainer { * latest path files. * Finally, adds the existing path files to the auto chooser */ - private void autoInit() { - try { - WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), - StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, - StandardWatchEventKinds.ENTRY_DELETE); - // TODO: Store this and other commands as fields so they can be rescheduled. - new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) { - @Override - public boolean runsWhenDisabled() { - return true; - } - }.withName("Path Watcher").schedule(); - } catch (IOException exception) { - LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); - } - Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()) - .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) - .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); - SmartDashboard.putData("Auto Chooser", autoChooser); - } + // private void autoInit() { + // try { + // WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), + // StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, + // StandardWatchEventKinds.ENTRY_DELETE); + // // TODO: Store this and other commands as fields so they can be rescheduled. + // new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) { + // @Override + // public boolean runsWhenDisabled() { + // return true; + // } + // }.withName("Path Watcher").schedule(); + // } catch (IOException exception) { + // LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); + // } + // Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()) + // .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) + // .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); + // SmartDashboard.putData("Auto Chooser", autoChooser); + // } /** * Creates a button on the SmartDashboard that will record the path of the * robot. */ - public void recordInit() { - SmartDashboard.putData("Recording", - new RunCommand(this::recordPeriodic) { - @Override - public void end(boolean interupted) { - new InstantCommand(RobotContainer.this::saveRecording) { - @Override - public boolean runsWhenDisabled() { - return true; - } - }.withName("Save Recording").schedule(); - } - }.withName("Record Path (Cancel to Save)")); - } + // public void recordInit() { + // SmartDashboard.putData("Recording", + // new RunCommand(this::recordPeriodic) { + // @Override + // public void end(boolean interupted) { + // new InstantCommand(RobotContainer.this::saveRecording) { + // @Override + // public boolean runsWhenDisabled() { + // return true; + // } + // }.withName("Save Recording").schedule(); + // } + // }.withName("Record Path (Cancel to Save)")); + // } - /** - * Called when a file is created, modified, or deleted. - * Adds newly created .path files to the SendableChooser. - * Reloads the path if the currently selected file is modified. - * - * @param watchKey The WatchKey that is being observed. - */ - private void updateAutoChooser(WatchKey watchKey) { - List> watchEvents = watchKey.pollEvents(); - if (!watchEvents.isEmpty()) { - List> pathWatchEvents = watchEvents.stream() - .filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList()); - for (WatchEvent pathWatchEvent : pathWatchEvents) { - Path watchEventPath = (Path) pathWatchEvent.context(); - File watchEventFile = watchEventPath.toFile(); - String watchEventFileName = watchEventFile.getName(); - if (watchEventFileName.endsWith(".path")) { - if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) { - LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", - watchEventFileName); - autoChooser.addOption(watchEventFile.getName(), watchEventFile); - } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) { - LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName); - if (watchEventFileName.equals(autoChooser.getSelected().getName())) { - LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName); - loadPath(watchEventFileName); - } - } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) { - LOGGER.log(Level.SEVERE, - "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", - watchEventFileName); - } - } - } - } - if (!watchKey.reset()) - LOGGER.severe("File watch key invalid."); - } + // /** + // * Called when a file is created, modified, or deleted. + // * Adds newly created .path files to the SendableChooser. + // * Reloads the path if the currently selected file is modified. + // * + // * @param watchKey The WatchKey that is being observed. + // */ + // private void updateAutoChooser(WatchKey watchKey) { + // List> watchEvents = watchKey.pollEvents(); + // if (!watchEvents.isEmpty()) { + // List> pathWatchEvents = watchEvents.stream() + // .filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList()); + // for (WatchEvent pathWatchEvent : pathWatchEvents) { + // Path watchEventPath = (Path) pathWatchEvent.context(); + // File watchEventFile = watchEventPath.toFile(); + // String watchEventFileName = watchEventFile.getName(); + // if (watchEventFileName.endsWith(".path")) { + // if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) { + // LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", + // watchEventFileName); + // autoChooser.addOption(watchEventFile.getName(), watchEventFile); + // } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) { + // LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName); + // if (watchEventFileName.equals(autoChooser.getSelected().getName())) { + // LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName); + // loadPath(watchEventFileName); + // } + // } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) { + // LOGGER.log(Level.SEVERE, + // "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", + // watchEventFileName); + // } + // } + // } + // } + // if (!watchKey.reset()) + // LOGGER.severe("File watch key invalid."); + // } - private void loadPath(String pathName) { - LOGGER.warning("Loading path " + pathName); - loadedPathTrajectory = null; - loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")), - SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION); - LOGGER.info("Done loading"); - } + // private void loadPath(String pathName) { + // LOGGER.warning("Loading path " + pathName); + // loadedPathTrajectory = null; + // loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")), + // SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION); + // LOGGER.info("Done loading"); + // } - private void saveRecording() { - // IMPORTANT: Had to chown the pathplanner folder in order to save autos. - File outputFile = PATHPLANNER_DIRECTORY - .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile(); - LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath()); - if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { - // TODO: Change to use measured maximum velocity and acceleration. - var path = createPath(null, null, false); - if (RobotBase.isReal()) - path.write(outputFile); - StringWriter writer = new StringWriter(); - path.write(writer); - recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString()); - autoChooser.setDefaultOption(outputFile.getName(), outputFile); - LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath()); - } else - LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); - } + // private void saveRecording() { + // // IMPORTANT: Had to chown the pathplanner folder in order to save autos. + // File outputFile = PATHPLANNER_DIRECTORY + // .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile(); + // LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath()); + // if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { + // // TODO: Change to use measured maximum velocity and acceleration. + // var path = createPath(null, null, false); + // if (RobotBase.isReal()) + // path.write(outputFile); + // StringWriter writer = new StringWriter(); + // path.write(writer); + // recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString()); + // autoChooser.setDefaultOption(outputFile.getName(), outputFile); + // LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath()); + // } else + // LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); + // } - public void recordPeriodic() { - Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation(); - Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d(); - // FIXME: Chassis speeds are created from joystick inputs and do not reflect - // actual robot velocity. - Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, - m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond); - Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, - SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false); - pathPoints.add(waypoint); - } + // // public void recordPeriodic() { + // // Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation(); + // // Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d(); + // // // FIXME: Chassis speeds are created from joystick inputs and do not reflect + // // // actual robot velocity. + // // Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, + // // m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond); + // // Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, + // // SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false); + // // pathPoints.add(waypoint); + // // } - public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) { - // Remove points whose angles to neighboring points are less than 10 degrees - // apart. - int j = 0; - for (int i = 1; i < pathPoints.size() - 1; i++) { - var prev = pathPoints.get(j).anchorPoint.orElseThrow(); - var current = pathPoints.get(i).anchorPoint.orElseThrow(); - var next = pathPoints.get(i + 1).anchorPoint.orElseThrow(); - var fromPrevious = current.minus(prev); - var toNext = next.minus(current); - var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY()); - var angleToNext = new Rotation2d(toNext.getX(), toNext.getY()); - if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE - || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE - && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false))) - pathPoints.set(i, null); - else - j = i; - } - pathPoints.removeIf(Objects::isNull); - // Make control points - pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(), - pathPoints.get(1).anchorPoint.orElseThrow()).getSecond()); - for (int i = 1; i < pathPoints.size() - 1; i++) { - var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(), - pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow()); - pathPoints.get(i).prevControl = Optional.of(controls.getFirst()); - pathPoints.get(i).nextControl = Optional.of(controls.getSecond()); - } - pathPoints.get(pathPoints.size() - 1).prevControl = Optional - .of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(), - pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst()); - // Create the path - PathPlannerUtil.Path path = new PathPlannerUtil.Path(); - path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new)); - path.maxVelocity = Optional.ofNullable(maxVelocity); - path.maxAcceleration = Optional.ofNullable(maxAcceleration); - path.isReversed = Optional.ofNullable(isReversed); - pathPoints.clear(); - return path; - } + // public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) { + // // Remove points whose angles to neighboring points are less than 10 degrees + // // apart. + // int j = 0; + // for (int i = 1; i < pathPoints.size() - 1; i++) { + // var prev = pathPoints.get(j).anchorPoint.orElseThrow(); + // var current = pathPoints.get(i).anchorPoint.orElseThrow(); + // var next = pathPoints.get(i + 1).anchorPoint.orElseThrow(); + // var fromPrevious = current.minus(prev); + // var toNext = next.minus(current); + // var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY()); + // var angleToNext = new Rotation2d(toNext.getX(), toNext.getY()); + // if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE + // || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE + // && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false))) + // pathPoints.set(i, null); + // else + // j = i; + // } + // pathPoints.removeIf(Objects::isNull); + // // Make control points + // pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(), + // pathPoints.get(1).anchorPoint.orElseThrow()).getSecond()); + // for (int i = 1; i < pathPoints.size() - 1; i++) { + // var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(), + // pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow()); + // pathPoints.get(i).prevControl = Optional.of(controls.getFirst()); + // pathPoints.get(i).nextControl = Optional.of(controls.getSecond()); + // } + // pathPoints.get(pathPoints.size() - 1).prevControl = Optional + // .of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(), + // pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst()); + // // Create the path + // PathPlannerUtil.Path path = new PathPlannerUtil.Path(); + // path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new)); + // path.maxVelocity = Optional.ofNullable(maxVelocity); + // path.maxAcceleration = Optional.ofNullable(maxAcceleration); + // path.isReversed = Optional.ofNullable(isReversed); + // pathPoints.clear(); + // return path; + // } - private static Pair makeControlPoints(Translation2d prev, Translation2d current, - Translation2d next) { - var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4); - return Pair.of(current.minus(line), current.plus(line)); - } + // private static Pair makeControlPoints(Translation2d prev, Translation2d current, + // Translation2d next) { + // var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4); + // return Pair.of(current.minus(line), current.plus(line)); + // } }