diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 62b8d36..3704a56 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -68,6 +68,8 @@ public class Robot extends TimedRobot { @Override public void disabledInit() { m_robotTime.endMatchTime(); + + // SmartDashboard.putBoolean("Cones?", true); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6747739..c1a95fd 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.CANSparkMax.FaultID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -80,6 +81,9 @@ public class RobotContainer { private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw); + private Command toggleClawCones = new InstantCommand(() -> m_robotClaw.toggleCones(), m_robotClaw); + private Command toggleClawCubes = new InstantCommand(() -> m_robotClaw.toggleCubes(), m_robotClaw); + public boolean readyForPlacement = true; private Boolean isPole = null; @@ -91,17 +95,7 @@ public class RobotContainer { new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04), new RotateToAngle(m_robotSwerveDrive, 0.0), new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) - // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape()) ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)); - - // private SequentialCommandGroup alignToShelf = - // new SequentialCommandGroup( - // new RotateToAngle(m_robotSwerveDrive, 0.0), - // new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), - // new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), - // new RotateToAngle(m_robotSwerveDrive, 0.0), - // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) - // ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); private SequentialCommandGroup alignToShelf = new SequentialCommandGroup( @@ -155,7 +149,7 @@ public class RobotContainer { ); private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 188), + new PivotCommand(m_robotArm, 185), new WaitCommand(0.3), new TeleCommand(m_robotArm, 29500) // toggleClaw.asProxy(), @@ -172,14 +166,6 @@ public class RobotContainer { /* Autos */ public SendableChooser chooser = new SendableChooser<>(); - // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); - - // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"); - // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - - // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1); - // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); private Command wait3 = new WaitCommand(3); @@ -230,28 +216,15 @@ public class RobotContainer { chooser.addOption("placeCubeMid", placeCubeMid); chooser.addOption("placeLow", placeLow); - // chooser.addOption("Blue1Path", blue1Path); - // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); - - // chooser.addOption("Red1Path", red1Path); - // chooser.addOption("Red1PathWithBalance", red1PathWithBalance); - playbackChooser = new PlaybackChooser(m_robotSwerveDrive) .addOption("PlaceRed1Balance", placeRed1Balance) .addOption("Wait3", wait3.asProxy()) .addOption("Wait5", wait5.asProxy()) .addOption("TaxiFar", taxiFar.asProxy()) - // .addOption("Red1Balance", new JoystickPlayback(m_robotSwerveDrive, "idk", 1)) .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)) .buildDisplay(); } - // here be dragons - enter if you dare - private static class TapState { - public boolean gearTapped = true; - public long gearTime = 0; - } - /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses @@ -261,38 +234,17 @@ public class RobotContainer { private void configureButtonBindings() { // * TEST BUTTONS - // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final - // * Driver Buttons new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final - // because closure reasons - ask Daniel Thomas - // final TapState tap = new TapState(); new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - // .onTrue(new InstantCommand(() -> { - // tap.gearTapped = true; - // tap.gearTime = System.currentTimeMillis(); - // })) - // .whileTrue(new RunCommand(() -> { - // if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) { - // m_robotSwerveDrive.setToTurbo(); - // tap.gearTapped = false; - // } - // })) - // .onFalse(new InstantCommand(() -> { - // if (tap.gearTapped) - // m_robotSwerveDrive.setToFast(); - // else - // m_robotSwerveDrive.setToSlow(); - // })); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final @@ -300,18 +252,18 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(interruptCommand.asProxy()); // final - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_TRIGGER_AXIS) - .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - () -> getDeadbandedDriverController().getLeftX(), - () -> getDeadbandedDriverController().getLeftY(), - () -> getDeadbandedDriverController().getRightX(), - () -> getDeadbandedDriverController().getRightY(), - "Red1Balance.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_TRIGGER_AXIS) + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // "Red1Balance.txt")) + // .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Red1Balance.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Red1Balance.txt")) + // .onFalse(new InstantCommand()); // * Operator Buttons @@ -328,8 +280,9 @@ public class RobotContainer { // toggle claw new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final .onTrue(toggleClaw.asProxy()); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotClaw.setAngle(45), m_robotClaw)); + // .onTrue(new ConditionalCommand(toggleClawCones.asProxy(), toggleClawCubes.asProxy(), () -> SmartDashboard.getBoolean("Cones?", true))); // kill soft limits new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index cc17eae..085a1b6 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -51,19 +51,28 @@ public class Claw extends SubsystemBase { // ! THIS IS FOR CUBE // m_leftMotor.setAngle(m_open ? 90 : 180); // m_rightMotor.setAngle(m_open ? 90 : 0); - - // if (m_open) - // m_spinnyspin.set(0.2); - // else - // m_spinnyspin.set(-0.2); - // new Timer().schedule(new TimerTask() { - // @Override - // public void run() { - // nospinnyspin(); - // } - // }, 750); } + public void setClawCones(boolean open) { + m_open = open; + m_leftMotor.setAngle(m_open ? 0 : 180); + m_rightMotor.setAngle(m_open ? 180 : 0); + } + + public void setClawCubes(boolean open) { + m_open = open; + m_leftMotor.setAngle(m_open ? 90 : 180); + m_rightMotor.setAngle(m_open ? 90 : 0); + } + + public void toggleCones() { + setClawCones(!m_open); + } + + public void toggleCubes() { + setClawCubes(!m_open); + } + public void toggle() { setClaw(!m_open); }