diff --git a/readme.md b/readme.md index 87bb837..1004199 100644 --- a/readme.md +++ b/readme.md @@ -10,15 +10,16 @@ - [x] Change SparkMax sendable implementation to be an extension. - [x] Add Shuffleboard layout plan. - [ ] Organize constants. -- [ ] Efficiently load path files. -- [ ] Change the path chooser to select autonomous commands. +- [x] Efficiently load path files. +- [x] Change the path chooser to select autonomous commands. - [ ] Remove unused commands. -- [ ] Rewrite controller bindings. -- [ ] Write new-style autonomous programs for lower ball counts. +- [x] Rewrite controller bindings. +- [x] Write new-style autonomous programs for lower ball counts. - [ ] Fix all XXX comments. - [ ] Visit TODO comments. - [ ] Forward the limelight port so it can be accessed over a USB tether. - [x] Remove turret offset from shooter tables. +- [ ] AutonomousBuilder *could* be static. ## Planned - [ ] Shuffleboard replacements options: - [ ] Run a web server on the robot and bypass network tables. diff --git a/shuffleboard.json b/shuffleboard.json index c2bef77..9162562 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -215,7 +215,7 @@ "_children": [ { "_type": "Graph", - "_source0": "network_table:///Robot/Values/Voltage", + "_source0": "network_table:///Robot/PDP/Voltage", "_title": "Graph", "Graph/Visible time": 15.0, "Graph/X-axis auto scrolling": true, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0a75d87..1d6f70d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,7 +15,6 @@ import edu.wpi.first.networktables.NTSendableBuilder; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.NetworkTableValue; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -26,21 +25,16 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandGroupBase; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc4388.robot.Constants.AutoConstants; +import edu.wpi.first.wpilibj2.command.button.POVButton; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.commands.PathPlannerCommand; +import frc4388.robot.commands.AutonomousBuilder; import frc4388.robot.commands.PathRecorder; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; -import frc4388.robot.commands.ExtenderIntakeCommands.RunExtender; -import frc4388.robot.commands.shooter.TimedWaitUntilCommand; import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shuffleboard.CommandSchedule; import frc4388.robot.commands.shuffleboard.ShooterTuner; @@ -59,13 +53,9 @@ import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.VisionOdometry; import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedXboxController; +import frc4388.utility.controller.XboxControllerPOV; import frc4388.utility.shuffleboard.CachingSendableChooser; -//TODO: Try using ConditionalCommand for subsystem default commands. -//TODO: Replace Path Recorder with Auto Chooser. -//TODO: Add POV button pad bindings as an example. -//XXX: Re-enable extender in autonomous. - /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -73,24 +63,22 @@ import frc4388.utility.shuffleboard.CachingSendableChooser; * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName()); - /* Robot Map */ public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final Camera m_robotCamera = new Camera("driver", 0, 160, 120, 40); - public final Climber m_robotClimber = new Climber(m_robotMap.elbow); - public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.frontLeft, m_robotMap.frontRight, m_robotMap.backLeft, m_robotMap.backRight, m_robotMap.gyro); - public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt); - public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); + public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); + public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt); public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); - public final LED m_robotLED = new LED(m_robotMap.LEDController); public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); + public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); + public final Climber m_robotClimber = new Climber(m_robotMap.elbow); + public final LED m_robotLED = new LED(m_robotMap.LEDController); + public final Camera m_robotCamera = new Camera("driver", 0, 160, 120, 40); public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); /* Dashboard Tools */ @@ -104,73 +92,301 @@ public class RobotContainer { private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); - private static boolean softLimits = true; + private boolean isClimberControlMode = false; + private boolean driveControlEnabled = true; - // control mode switching - private enum ControlMode { - SHOOTER, CLIMBER - } - - private ControlMode currentControlMode = ControlMode.SHOOTER; - - // drive on off mode switching - private enum DriveMode { - ON, OFF - } - - private DriveMode currentDriveMode = DriveMode.ON; - - - - private final Map tablesToData = new HashMap<>(); - private final NetworkTable networkTable = NetworkTableInstance.getDefault().getTable("Robot"); - - private synchronized void putData(String key, Sendable data) { - Sendable sddata = tablesToData.get(key); - if (sddata == null || sddata != data) { - tablesToData.put(key, data); - NetworkTable dataTable = networkTable.getSubTable(key); - SendableBuilderImpl builder = new SendableBuilderImpl(); - builder.setTable(dataTable); - SendableRegistry.publish(data, builder); - builder.startListeners(); - dataTable.getEntry(".name").setString(key); - } - } - - public synchronized void updateValues() { - for (Sendable data : tablesToData.values()) { - SendableRegistry.update(data); - } - } - - - /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - m_autoChooser.addOption("OneBall", this::makeOneBallCommand); - m_autoChooser.addOption("TwoBall", this::makeTwoBallCommand); - m_autoChooser.setDefaultOption("ThreeBall", this::makeThreeBallCommand); + configureButtonBindings(); + + configureDefaultCommands(); + + AutonomousBuilder autoBuilder = new AutonomousBuilder(this); + m_autoChooser.addOption("OneBall", autoBuilder::buildOneBallCommand); + m_autoChooser.addOption("TwoBall", autoBuilder::buildTwoBallCommand); + m_autoChooser.setDefaultOption("ThreeBall", autoBuilder::buildThreeBallCommand); SmartDashboard.putData("Autonomous", m_autoChooser); SmartDashboard.putData("Path Recorder", m_pathRecorder); + SmartDashboard.putData("Shooter Tuner", m_shooterTuner); + SmartDashboard.putData("Command Schedule", m_commandSchedule); - // Preferences.initString("Autonomous", "Three Ball"); + putRobotDashboardData(); + } - - - var pdp = new PowerDistribution() { - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.setSmartDashboardType("PowerDistributionPanel"); + /* Default Commands */ + private void configureDefaultCommands() { + // Swerve Drive with Input + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + if (driveControlEnabled) { + m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), getDriverController().getRight(), true); + } else { + m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false); } - }; + }, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); + + // Intake with Triggers + m_robotIntake.setDefaultCommand(new RunCommand(() -> m_robotIntake.runWithTriggers(getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake DefaultCommand")); + + // Shooter Idle + m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom).withName("BoomBoom DefaultCommand")); + + // Serializer Manual + m_robotSerializer.setDefaultCommand(new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8), m_robotSerializer).withName("Serializer DefaultCommand")); + + // Turret Manual + m_robotTurret.setDefaultCommand(new RunCommand(() -> { + if (!isClimberControlMode) { + m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); + } else { + m_robotTurret.runTurretWithInput(0); + } + }, m_robotTurret).withName("Turret DefaultCommand")); + + // Hood Manual + m_robotHood.setDefaultCommand(new RunCommand(() -> { + if (!isClimberControlMode) { + m_robotHood.runHood(getOperatorController().getLeftY()); + } else { + m_robotHood.runHood(0); + } + }, m_robotHood).withName("Hood DefaultCommand")); + + // Climber Manual + m_robotClimber.setDefaultCommand(new RunCommand(() -> { + if (!isClimberControlMode) { + m_robotClimber.setMotors(0.0); + } else { + m_robotClimber.setMotors(-getOperatorController().getRightY()); + } + }, m_robotClimber).withName("Climber DefaultCommand")); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by instantiating + * a {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or + * {@link XboxController}), and then passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + configureDriverButtonBindings(); + configureOperatorButtonBindings(); + configureBoxButtonBindings(); + } + + private void configureDriverButtonBindings() { + // Iterate over and assign commands to all Xbox controller buttons. + for (XboxController.Button binding : XboxController.Button.values()) { + /* ------------------------------------ Driver ------------------------------------ */ + JoystickButton button = new JoystickButton(getDriverController(), binding.value); + if (binding == XboxController.Button.kLeftBumper) { + /* Left Bumper > Shift Down */ + button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + } else if (binding == XboxController.Button.kRightBumper) { + /* Right Bumper > Shift Up */ + button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + } else if (binding == XboxController.Button.kLeftStick) { + /* Left Stick > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxController.Button.kRightStick) { + /* Right Stick > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxController.Button.kA) { + /* A > Unbound */ + button.whenPressed(this::switchControlMode); + button.whenReleased(this::switchControlMode); + } else if (binding == XboxController.Button.kB) { + /* B > Unbound */ + button.whenPressed(this::switchDriveMode); + button.whenReleased(this::switchDriveMode); + } else if (binding == XboxController.Button.kX) { + /* X > Unbound */ + button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)); + button.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + } else if (binding == XboxController.Button.kY) { + /* Y > Unbound */ + button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)); + button.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + } else if (binding == XboxController.Button.kBack) { + /* Back > Calibrate Odometry */ + button.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); + } else if (binding == XboxController.Button.kStart) { + /* Start > Calibrate Odometry */ + button.whenPressed(m_robotSwerveDrive::resetGyro); + } + } + // Iterate over and assign commands to cardinal Xbox controller d-pad directions. + for (XboxControllerPOV binding : XboxControllerPOV.values()) { + POVButton button = new POVButton(getDriverController(), binding.value); + if (binding == XboxControllerPOV.kUp) { + /* D-pad Up > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxControllerPOV.kRight) { + /* D-pad Right > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxControllerPOV.kDown) { + /* D-pad Down > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxControllerPOV.kLeft) { + /* D-pad Left > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } + } + } + + private void configureOperatorButtonBindings() { + // Iterate over and assign commands to all Xbox controller buttons. + for (XboxController.Button binding : XboxController.Button.values()) { + /* ------------------------------------ Operator ------------------------------------ */ + JoystickButton button = new JoystickButton(getDriverController(), binding.value); + if (binding == XboxController.Button.kLeftBumper) { + /* Left Bumper > Storage In */ + button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))); + button.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + } else if (binding == XboxController.Button.kRightBumper) { + /* Right Bumper > Storage Out */ + button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))); + button.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + } else if (binding == XboxController.Button.kLeftStick) { + /* Left Stick > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxController.Button.kRightStick) { + /* Right Stick > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxController.Button.kA) { + /* A > Spit Out Ball */ + button.whileHeld(new RunCommand(m_robotTurret::gotoMidpoint, m_robotTurret)); + button.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); + } else if (binding == XboxController.Button.kB) { + /* B > Toggle Claws */ + button.whenPressed(m_robotClaws::toggleClaws, m_robotClaws); + } else if (binding == XboxController.Button.kX) { + /* X > Toggle Extender Deployment */ + button.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + } else if (binding == XboxController.Button.kY) { + /* Y > Track Target */ + button.whileHeld(new TrackTarget(m_robotVisionOdometry, m_robotTurret, m_robotHood, m_robotBoomBoom)); + } else if (binding == XboxController.Button.kBack) { + /* Back > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxController.Button.kStart) { + /* Start > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } + } + // Iterate over and assign commands to cardinal Xbox controller d-pad directions. + for (XboxControllerPOV binding : XboxControllerPOV.values()) { + POVButton button = new POVButton(getDriverController(), binding.value); + if (binding == XboxControllerPOV.kUp) { + /* D-pad Up > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxControllerPOV.kRight) { + /* D-pad Right > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxControllerPOV.kDown) { + /* D-pad Down > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == XboxControllerPOV.kLeft) { + /* D-pad Left > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } + } + } + + private void configureBoxButtonBindings() { + // Iterate over and assign commands to all button box buttons. + for (ButtonBox.Button binding : ButtonBox.Button.values()) { + /* ---------------------------------- Button Box ---------------------------------- */ + JoystickButton button = new JoystickButton(getButtonBox(), binding.value); + if (binding == ButtonBox.Button.kRightSwitch) { + /* Right Switch > Unbound */ + button.whenPressed(new PrintCommand("Unbound")); + } else if (binding == ButtonBox.Button.kMiddleSwitch) { + /* Middle Switch > Climber and Shooter mode switching */ + button.whenPressed(() -> isClimberControlMode = true); + button.whenReleased(() -> isClimberControlMode = false); + } else if (binding == ButtonBox.Button.kLeftSwitch) { + /* Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender) */ + button.whenPressed(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret); + button.whenPressed(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret); + + button.whenPressed(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood); + button.whenPressed(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood); + + button.whenPressed(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender); + + button.whenReleased(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret); + button.whenReleased(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret); + + button.whenReleased(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood); + button.whenReleased(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood); + + button.whenReleased(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender); + + button.whenReleased(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret); + button.whenReleased(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood); + button.whenReleased(() -> m_robotExtender.setEncoder(0), m_robotExtender); + button.whenReleased(ExtenderIntakeGroup::setDirectionToOut, m_robotIntake, m_robotExtender); + button.whenReleased(() -> m_robotClimber.setEncoders(0), m_robotClimber); + } else if (binding == ButtonBox.Button.kRightButton) { + /* Right Button > Extender Out */ + button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)); + button.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + } else if (binding == ButtonBox.Button.kLeftButton) { + /* Left Button > Extender In */ + button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)); + button.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + } + } + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return m_autoChooser.getSelected(); + } + + public boolean isLockedOn() { + return m_robotTurret.isLockedOn() || m_robotHood.isLockedOn() || m_robotBoomBoom.isLockedOn(); + } + + public void switchControlMode() { + isClimberControlMode = !isClimberControlMode; + } + + public void switchDriveMode() { + driveControlEnabled = !driveControlEnabled; + } + + public DeadbandedXboxController getDriverController() { + return m_driverXbox; + } + + public DeadbandedXboxController getOperatorController() { + return m_operatorXbox; + } + + public ButtonBox getButtonBox() { + return m_buttonBox; + } + + /** + * Set odometry to given pose. + * + * @param pose Pose to set odometry to. + */ + public void resetOdometry(Pose2d pose) { + m_robotSwerveDrive.resetOdometry(pose); + } + + private void putRobotDashboardData() { putData("Values", builder -> { builder.addDoubleProperty("Match Time", DriverStation::getMatchTime, null); - builder.addDoubleProperty("Voltage", pdp::getVoltage, null); builder.addDoubleProperty("Claws", m_robotClaws.m_rightClaw::get, null); builder.addDoubleProperty("Arm", m_robotMap.elbow::get, null); builder.addDoubleProperty("Intake", m_robotMap.intakeMotor::get, null); @@ -182,7 +398,13 @@ public class RobotContainer { builder.addBooleanProperty("Shooter Safety", this::isLockedOn, null); }); putData("Field", m_robotSwerveDrive.m_field); - putData("PDP", pdp); + putData("PDP", new PowerDistribution() { + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.setSmartDashboardType("PowerDistributionPanel"); + } + }); putData("Extender", new NTSendable() { @Override public void initSendable(NTSendableBuilder builder) { @@ -224,318 +446,25 @@ public class RobotContainer { } }); putData("Drive Camera", m_robotCamera); - - - - - - configureButtonBindings(); - /* Default Commands */ - // Swerve Drive with Input - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - if (currentDriveMode == DriveMode.ON) { - m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), getDriverController().getRight(), true); - } - if (currentDriveMode == DriveMode.OFF) { - m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false); - } - }, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); - - // Intake with Triggers - m_robotIntake.setDefaultCommand(new RunCommand(() -> m_robotIntake.runWithTriggers(getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake DefaultCommand")); - - // TODO: Comment - m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom).withName("BoomBoom DefaultCommand")); - - // Serializer Manual - m_robotSerializer.setDefaultCommand(new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8), m_robotSerializer).withName("Serializer DefaultCommand")); - - // Turret Manual - m_robotTurret.setDefaultCommand(new RunCommand(() -> { - if (currentControlMode == ControlMode.SHOOTER) { - m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); - } - if (currentControlMode == ControlMode.CLIMBER) { - m_robotTurret.runTurretWithInput(0); - } - }, m_robotTurret)); - - // Hood Manual - m_robotHood.setDefaultCommand(new RunCommand(() -> { - if (currentControlMode == ControlMode.SHOOTER) { - m_robotHood.runHood(getOperatorController().getLeftY()); - } - if (currentControlMode == ControlMode.CLIMBER) { - m_robotHood.runHood(0); - } - }, m_robotHood)); - - // Climber Manual - m_robotClimber.setDefaultCommand(new RunCommand(() -> { - if (currentControlMode == ControlMode.SHOOTER) { - m_robotClimber.setMotors(0.0); - } - if (currentControlMode == ControlMode.CLIMBER) { - m_robotClimber.setMotors(-getOperatorController().getRightY()); - } - }, m_robotClimber)); - - SmartDashboard.putData("Shooter Tuner", m_shooterTuner); - SmartDashboard.putData("Shooter Tuner", m_shooterTuner); - SmartDashboard.putData("Command Schedule", m_commandSchedule); } - /** - * Use this method to define your button->command mappings. Buttons can be created by instantiating - * a {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or - * {@link XboxController}), and then passing it to a - * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - // ! Driver Buttons + private final Map m_tablesToData = new HashMap<>(); + private final NetworkTable m_networkTable = NetworkTableInstance.getDefault().getTable("Robot"); - // Start > Calibrate Odometry - new JoystickButton(getDriverController(), XboxController.Button.kBack.value).whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - - // Start > Calibrate Odometry - new JoystickButton(getDriverController(), XboxController.Button.kStart.value).whenPressed(m_robotSwerveDrive::resetGyro); - - // Left Bumper > Shift Down - new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value).whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); - - // Right Bumper > Shift Up - new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value).whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - - new JoystickButton(getDriverController(), XboxController.Button.kA.value).whenPressed(new InstantCommand(() -> switchControlMode())).whenReleased(new InstantCommand(() -> switchControlMode())); - - new JoystickButton(getDriverController(), XboxController.Button.kB.value).whenPressed(new InstantCommand(() -> switchDriveMode())).whenReleased(new InstantCommand(() -> switchDriveMode())); - - // ! Operator Buttons - - // Right Bumper > Storage Out - new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value).whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))).whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - - // Left Bumper > Storage In - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value).whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))).whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - - // B > Toggle claws - new JoystickButton(getOperatorController(), XboxController.Button.kB.value).whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); - - // X > Toggles extender in and out - new JoystickButton(getOperatorController(), XboxController.Button.kX.value).whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); - - // A > Spit Out Ball - new JoystickButton(getOperatorController(), XboxController.Button.kA.value).whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)).whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); - - // Y > Full aim command - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, - // m_robotVisionOdometry)); - - // ! Test Buttons - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, - // m_robotVisionOdometry, false, false)); - - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whenPressed(new RunCommandForTime(new RunCommand(() -> - // m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), - // m_robotTurret), 1.0)); - new JoystickButton(getOperatorController(), XboxController.Button.kY.value).whileHeld(new TrackTarget(m_robotVisionOdometry, m_robotTurret, m_robotHood, m_robotBoomBoom)); // * aim with turret to target); - - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); - - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); - - // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); - - // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) - // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - - // ! Button Box Buttons - // Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, - // climber, and extender) - - // SmartDashboard.putData("BB LEFT ON", new SequentialCommandGroup( - // new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret), - // new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret), - - // new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood), - // new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood), - - // new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender) - // )); - - // SmartDashboard.putData("BB LEFT OFF", new SequentialCommandGroup( - // new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret), - // new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret), - - // new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood), - // new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood), - - // new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender), - - // new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret), - // new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood), - // new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender), - // new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, - // m_robotExtender), - // new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber) - // )); - - // new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) - // .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) - // .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) - - // .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) - // .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) - - // .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), - // m_robotExtender)) - - // .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) - // .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) - - // .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) - // .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) - - // .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), - // m_robotExtender)) - - // .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), - // m_robotTurret)) - // .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) - // .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) - // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, - // m_robotExtender)) - // .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); - - // Middle Switch > Climber and Shooter mode switching - // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER)) - // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); - // // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - // // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.)) - // // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); - - // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - // .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF)) - // .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON)); - - // // Left Button > Extender In - new JoystickButton(getDriverController(), XboxController.Button.kX.value).whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)).whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - - // Left Button > Extender Out - new JoystickButton(getDriverController(), XboxController.Button.kY.value).whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)).whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + private synchronized void putData(String key, Sendable data) { + Sendable sddata = m_tablesToData.get(key); + if (sddata == null || sddata != data) { + m_tablesToData.put(key, data); + NetworkTable dataTable = m_networkTable.getSubTable(key); + SendableBuilderImpl builder = new SendableBuilderImpl(); + builder.setTable(dataTable); + SendableRegistry.publish(data, builder); + builder.startListeners(); + dataTable.getEntry(".name").setString(key); + } } - public boolean isLockedOn() { - return m_robotTurret.isLockedOn() || m_robotHood.isLockedOn() || m_robotBoomBoom.isLockedOn(); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return m_autoChooser.getSelected(); - } - - private Command makeOneBallCommand() { - return CommandGroupBase.sequence( - makeStartupCommandPart(), - // Shoot Preloaded Ball - makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_ONE_BALL, "FirstBall"), - makeStopCommandPart() - ).withName("OneBall"); - } - - private Command makeTwoBallCommand() { - return CommandGroupBase.sequence( - makeStartupCommandPart(), - // Get Second Ball - new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"), - new PathPlannerCommand("JMove1", AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, m_robotSwerveDrive).withName("JMove1"), - // Shoot Preloaded and Second Ball - makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_TWO_BALLS, "FirstSecondBall"), - makeStopCommandPart() - ).withName("TwoBall"); - } - - private Command makeThreeBallCommand() { - return CommandGroupBase.sequence( - makeStartupCommandPart(), - // Get Second Ball - new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"), - new PathPlannerCommand("JMove1", AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, m_robotSwerveDrive).withName("JMove1"), - // Shoot Preloaded and Second Ball - makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_TWO_BALLS, "FirstSecondBall"), - // Get Third Ball - new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), - new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"), - new PathPlannerCommand("JMove2", AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, m_robotSwerveDrive).withName("JMove2"), - // Shoot Third Ball - makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_ONE_BALL, "ThirdBall"), - makeStopCommandPart() - ).withName("ThreeBall"); - } - - private Command makeStartupCommandPart() { - return CommandGroupBase.sequence( - new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"), - new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake") - // new RunExtender(m_robotExtender).withName("DeployExtender") - ); - } - - private Command makeStopCommandPart() { - return CommandGroupBase.sequence( - new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StopRunningIntake"), - new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"), - new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("StopRunningStorage") - ); - } - - private ParallelDeadlineGroup makeTimeoutTrackShotGroup(double lockOnDuration, double lockOnTimeAllowance, double storageRunTime, String name) { - return CommandGroupBase.sequence( - new TimedWaitUntilCommand(this::isLockedOn, lockOnDuration).withTimeout(lockOnTimeAllowance).withName(name + "LockOn"), - new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage).withName(name + "Feed"), - new WaitCommand(storageRunTime).withName(name + "ShootTimer"), - new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName(name + "StopFeed")).deadlineWith(new TrackTarget(m_robotVisionOdometry, m_robotTurret, m_robotHood, m_robotBoomBoom).withName(name + "Track") - ); - } - - public void switchControlMode() { - currentControlMode = currentControlMode == ControlMode.SHOOTER ? ControlMode.CLIMBER : ControlMode.SHOOTER; - } - - public void switchDriveMode() { - currentDriveMode = currentDriveMode == DriveMode.ON ? DriveMode.OFF : DriveMode.ON; - } - - public DeadbandedXboxController getDriverController() { - return m_driverXbox; - } - - public DeadbandedXboxController getOperatorController() { - return m_operatorXbox; - } - - public ButtonBox getButtonBox() { - return m_buttonBox; - } - - /** - * Set odometry to given pose. - * - * @param pose Pose to set odometry to. - */ - public void resetOdometry(Pose2d pose) { - m_robotSwerveDrive.resetOdometry(pose); + public synchronized void updateValues() { + m_tablesToData.values().forEach(SendableRegistry::update); } } diff --git a/src/main/java/frc4388/robot/commands/AutonomousBuilder.java b/src/main/java/frc4388/robot/commands/AutonomousBuilder.java new file mode 100644 index 0000000..20eda51 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AutonomousBuilder.java @@ -0,0 +1,109 @@ +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandGroupBase; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc4388.robot.RobotContainer; +import frc4388.robot.Constants.AutoConstants; +import frc4388.robot.Constants.StorageConstants; +import frc4388.robot.commands.shooter.TimedWaitUntilCommand; +import frc4388.robot.commands.shooter.TrackTarget; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Extender; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Serializer; +import frc4388.robot.subsystems.Storage; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; + +public class AutonomousBuilder { + private final RobotContainer m_robotContainer; + private final SwerveDrive m_drive; + private final Extender m_extender; + private final Intake m_intake; + private final Serializer m_serializer; + private final Storage m_storage; + private final BoomBoom m_boomBoom; + private final Turret m_turret; + private final Hood m_hood; + private final VisionOdometry m_visionOdometry; + public AutonomousBuilder(RobotContainer robotContainer) { + m_robotContainer = robotContainer; + m_drive = m_robotContainer.m_robotSwerveDrive; + m_extender = m_robotContainer.m_robotExtender; + m_intake = m_robotContainer.m_robotIntake; + m_serializer = m_robotContainer.m_robotSerializer; + m_storage = m_robotContainer.m_robotStorage; + m_boomBoom = m_robotContainer.m_robotBoomBoom; + m_turret = m_robotContainer.m_robotTurret; + m_hood = m_robotContainer.m_robotHood; + m_visionOdometry = m_robotContainer.m_robotVisionOdometry; + } + public Command buildOneBallCommand() { + return CommandGroupBase.sequence( + buildStartupCommandPart(), + // Shoot Preloaded Ball + buildTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_ONE_BALL, "FirstBall"), + buildStopCommandPart() + ).withName("OneBall"); + } + + public Command buildTwoBallCommand() { + return CommandGroupBase.sequence( + buildStartupCommandPart(), + // Get Second Ball + new InstantCommand(() -> m_turret.runShooterRotatePID(-180), m_turret).withName("StartTurningShooter"), + new PathPlannerCommand("JMove1", AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, m_drive).withName("JMove1"), + // Shoot Preloaded and Second Ball + buildTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_TWO_BALLS, "FirstSecondBall"), + buildStopCommandPart() + ).withName("TwoBall"); + } + + public Command buildThreeBallCommand() { + return CommandGroupBase.sequence( + buildStartupCommandPart(), + // Get Second Ball + new InstantCommand(() -> m_turret.runShooterRotatePID(-180), m_turret).withName("StartTurningShooter"), + new PathPlannerCommand("JMove1", AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, m_drive).withName("JMove1"), + // Shoot Preloaded and Second Ball + buildTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_TWO_BALLS, "FirstSecondBall"), + // Get Third Ball + new InstantCommand(() -> m_boomBoom.runDrumShooterVelocityPID(8000), m_boomBoom).withName("StartIdlingShooter"), + new InstantCommand(() -> m_turret.runShooterRotatePID(-120), m_turret).withName("StartTurningShooter"), + new PathPlannerCommand("JMove2", AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, m_drive).withName("JMove2"), + // Shoot Third Ball + buildTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_ONE_BALL, "ThirdBall"), + buildStopCommandPart() + ).withName("ThreeBall"); + } + + private Command buildStartupCommandPart() { + return CommandGroupBase.sequence( + new InstantCommand(() -> m_boomBoom.runDrumShooterVelocityPID(8000), m_boomBoom).withName("StartIdlingShooter"), + new InstantCommand(() -> m_intake.runAtOutput(-1), m_intake).withName("StartRunningIntake") + // new RunExtender(m_extender).withName("DeployExtender") + ); + } + + private Command buildStopCommandPart() { + return CommandGroupBase.sequence( + new InstantCommand(() -> m_intake.runAtOutput(0), m_intake).withName("StopRunningIntake"), + new InstantCommand(() -> m_serializer.setSerializer(0.0), m_serializer).withName("StopRunningSerializer"), + new InstantCommand(() -> m_storage.runStorage(0.0)).withName("StopRunningStorage") + ); + } + + private ParallelDeadlineGroup buildTimeoutTrackShotGroup(double lockOnDuration, double lockOnTimeAllowance, double storageRunTime, String name) { + return CommandGroupBase.sequence( + new TimedWaitUntilCommand(m_robotContainer::isLockedOn, lockOnDuration).withTimeout(lockOnTimeAllowance).withName(name + "LockOn"), + new InstantCommand(() -> m_storage.runStorage(StorageConstants.STORAGE_SPEED), m_storage).withName(name + "Feed"), + new WaitCommand(storageRunTime).withName(name + "ShootTimer"), + new InstantCommand(() -> m_storage.runStorage(0.0)).withName(name + "StopFeed") + ).deadlineWith(new TrackTarget(m_visionOdometry, m_turret, m_hood, m_boomBoom).withName(name + "Track")); + } +} diff --git a/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java b/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java index db6c3f1..eddea6e 100644 --- a/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java +++ b/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java @@ -22,6 +22,8 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -53,11 +55,11 @@ public class ShooterTuner extends CommandBase { @Override public void initialize() { - var tab = Shuffleboard.getTab("Shooter Tuner"); + ShuffleboardTab tab = Shuffleboard.getTab("Shooter Tuner"); if (tab.getComponents().isEmpty()) { - var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 5); + ShuffleboardLayout manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 5); //XXX: - var sbi = new SendableBuilderImpl(); + SendableBuilderImpl sbi = new SendableBuilderImpl(); sbi.setTable(NetworkTableInstance.getDefault().getTable("ShooterTuner/Manual Shooter Data")); sbi.addDoubleProperty("Drum Velocity", () -> tableOverrideEntry.drumVelocity, d -> tableOverrideEntry.drumVelocity = d); sbi.addDoubleProperty("Hood Extension", () -> tableOverrideEntry.hoodExt, d -> tableOverrideEntry.hoodExt = d); @@ -65,7 +67,7 @@ public class ShooterTuner extends CommandBase { sbi.addBooleanProperty("Measure Distance", () -> measureDistance, b -> measureDistance = b); // manual.add("Manual Shooter Data", m_shotEditor); manual.add("Manual Data Appender", m_shotCsvAppender); - var csv = tab.getLayout("Shooter Table", BuiltInLayouts.kList).withPosition(2, 0).withSize(7, 5); + ShuffleboardLayout csv = tab.getLayout("Shooter Table", BuiltInLayouts.kList).withPosition(2, 0).withSize(7, 5); csv.add("Shooter Table", m_tableEditor); csv.add("Save to CSV File", new InstantCommand(m_boomBoom::saveShooterTable) { @Override diff --git a/src/main/java/frc4388/utility/controller/XboxControllerPOV.java b/src/main/java/frc4388/utility/controller/XboxControllerPOV.java new file mode 100644 index 0000000..38b958f --- /dev/null +++ b/src/main/java/frc4388/utility/controller/XboxControllerPOV.java @@ -0,0 +1,25 @@ +package frc4388.utility.controller; + +/** Represents a d-pad angle on an XboxController. */ +public enum XboxControllerPOV { + kUp(0), + kUpRight(45), + kRight(90), + kDownRight(135), + kDown(180), + kDownLeft(225), + kLeft(270), + kUpLeft(315); + + @SuppressWarnings("MemberName") + public final int value; + + XboxControllerPOV(int value) { + this.value = value; + } + + @Override + public String toString() { + return this.name().substring(1) + "Button"; + } +}