From 1b6265c65adf858ad7cd818dd74e0aa98a6bbab8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 14:26:03 -0600 Subject: [PATCH] new try at turret or climber thing --- src/main/java/frc4388/robot/Constants.java | 1 + .../java/frc4388/robot/RobotContainer.java | 45 +++++-------------- .../robot/subsystems/ClimberRewrite.java | 7 +-- 3 files changed, 13 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7a06fd9..b2ba4d6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,6 +175,7 @@ public final class Constants { public static final int GYRO_ID = 14; public static final double INPUT_MULTIPLIER = 0.7; + public static final double CLIMBER_SOFT_LIMIT_TOLERANCE = 20.0; // 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 5cb54c8..29d213b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,14 +112,10 @@ public class RobotContainer { /* Subsystems */ public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); -<<<<<<< Updated upstream private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); // Subsystems -======= - public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); ->>>>>>> Stashed changes public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); @@ -203,7 +199,6 @@ public class RobotContainer { m_robotSerializer.setDefaultCommand( new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); -<<<<<<< Updated upstream // Turret Manual @@ -217,12 +212,6 @@ public class RobotContainer { .until(() -> this.currentMode.equals(CurrentMode.TURRET))); // EnableTurret() -======= - // Turret Manual - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); ->>>>>>> Stashed changes // m_robotHood.setDefaultCommand( // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); @@ -330,8 +319,17 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) - .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); + + .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) + .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftY(), getOperatorController().getRightY()), m_robotClimber)))) + + .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null))) + .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand( + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)))); + + // .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) + // .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); // .whenReleased(EnableClimber())); // control turret manual mode @@ -425,26 +423,6 @@ public class RobotContainer { * Finally, adds the existing path files to the auto chooser */ private void autoInit() { -<<<<<<< Updated upstream - // 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); -======= try { WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, @@ -463,7 +441,6 @@ public class RobotContainer { .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); SmartDashboard.putData("Auto Chooser", autoChooser); ->>>>>>> Stashed changes } /** diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index a9d208a..621b213 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -179,6 +179,7 @@ public class ClimberRewrite extends SubsystemBase { SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); // setJointAngles(jointAngles); + } /** @@ -286,11 +287,6 @@ public class ClimberRewrite extends SubsystemBase { return getClimberPosition(jointAngles[0], jointAngles[1]); } -<<<<<<< Updated upstream - public double getCurrent() { - return (this.m_elbow.getSupplyCurrent() + this.m_shoulder.getSupplyCurrent()); - } -======= public void setClimberSoftLimits(boolean set){ m_elbow.configForwardSoftLimitEnable(set); m_shoulder.configForwardSoftLimitEnable(set); @@ -305,5 +301,4 @@ public class ClimberRewrite extends SubsystemBase { return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); } ->>>>>>> Stashed changes }