From 1d9940f81b1c09429406b083758100cfb697c2db Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 14:09:25 -0600 Subject: [PATCH] gAtEkEePiNg --- src/main/java/frc4388/robot/Constants.java | 2 + .../java/frc4388/robot/RobotContainer.java | 211 +++++++++--------- .../robot/subsystems/ClimberRewrite.java | 8 +- 3 files changed, 114 insertions(+), 107 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7930670..d9bf54d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -173,6 +173,8 @@ public final class Constants { public static final int SHOULDER_ID = 30; public static final int ELBOW_ID = 31; public static final int GYRO_ID = 14; + + public static final double INPUT_MULTIPLIER = 0.7; // 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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 791fcbc..5b4f7b7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -22,6 +22,7 @@ import java.util.Comparator; import java.util.List; import java.util.Objects; import java.util.Optional; +import java.util.function.BooleanSupplier; import java.util.function.Function; import java.util.logging.Level; import java.util.logging.Logger; @@ -64,9 +65,6 @@ import frc4388.robot.subsystems.Claws; import frc4388.robot.commands.RunClaw; import frc4388.robot.subsystems.ClimberRewrite; import frc4388.robot.subsystems.Claws.ClawType; -import frc4388.robot.commands.AimToCenter; -import frc4388.robot.commands.Shoot; -import frc4388.robot.commands.TrackTarget; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; @@ -113,7 +111,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); @@ -130,8 +128,8 @@ public class RobotContainer { public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); - private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); + // private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); + // private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); //public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); // Controllers @@ -156,19 +154,15 @@ public class RobotContainer { public static boolean softLimits = true; + public static enum CurrentMode {TURRET, CLIMBER}; + public CurrentMode currentMode = CurrentMode.TURRET; + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureButtonBindings(); /* Default Commands */ - - // moves climber in xy space with two-axis input from the operator controller - m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.7, getOperatorController().getRightY() * 0.7), - m_robotClimber)); - - // IK command // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), @@ -196,21 +190,28 @@ public class RobotContainer { // m_robotBoomBoom, // m_robotTurret).withName("Storage ManageStorage defaultCommand")); - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) - // ); - // Storage Management - /*m_robotStorage.setDefaultCommand( - new RunCommand(() -> m_robotStorage.manageStorage(), - m_robotStorage).withName("Storage manageStorage 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.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), + new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); - // Turret Manual + + // Turret Manual + m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret) + .until(() -> this.currentMode.equals(CurrentMode.CLIMBER))); + // .withName("Turret runTurretWithInput defaultCommand"); + + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.setMotors(getDriverController().getLeftY(), getDriverController().getRightY()), m_robotClimber) + .until(() -> this.currentMode.equals(CurrentMode.TURRET))); + + // EnableTurret() m_robotHood.setDefaultCommand( new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); @@ -247,10 +248,6 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - - - - /* Operator Buttons */ new JoystickButton(getOperatorController(), XboxController.Button.kY.value) @@ -262,9 +259,6 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); - - - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); @@ -321,15 +315,16 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); - // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - // .whileHeld(new TurretManual(m_robotTurret)); + new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); + // .whenReleased(EnableClimber())); // control turret manual mode // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); - new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); @@ -339,6 +334,12 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } + //EnableTurret() + //set turret default to joysticks, set climber default to null + + //EnableClimber() + //set climber default to joysticks set turret defaults to null + /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -389,18 +390,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); + } /** * Creates a WatchKey for the path planner directory and registers it with the @@ -409,26 +410,26 @@ 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 @@ -458,35 +459,35 @@ public class RobotContainer { * @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."); + // 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) { @@ -498,22 +499,22 @@ public class RobotContainer { } 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()); + // // 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() { diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index ef6e973..4d81b38 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -128,8 +128,8 @@ public class ClimberRewrite extends SubsystemBase { } public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput); - m_elbow.set(elbowOutput); + m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER); + m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER); } public double[] getJointAngles() { @@ -282,4 +282,8 @@ public class ClimberRewrite extends SubsystemBase { public static Point getClimberPosition(double[] jointAngles) { return getClimberPosition(jointAngles[0], jointAngles[1]); } + + public double getCurrent() { + return (this.m_elbow.getSupplyCurrent() + this.m_shoulder.getSupplyCurrent()); + } }