diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dc08c37..76f633f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -49,10 +49,6 @@ public class RobotContainer { private final Shooter shooter; private final Command intakeCommand, shootCommand, aimAtLimelightCommand, rotateCommand, driveToPoseCommand, aimAndPickUpNoteCommand; - // CommandJoystick rotationController = new CommandJoystick(1); - // Replace with CommandPS4Controller or CommandJoystick if needed - - // CommandJoystick driverController = new CommandJoystick(3);//(OperatorConstants.DRIVER_CONTROLLER_PORT); XboxController driverXbox = new XboxController(0); XboxController shooterXbox = new XboxController(1); @@ -124,9 +120,6 @@ private void configureBindings() { new JoystickButton(driverXbox, XboxController.Button.kX.value).whileTrue(aimAtLimelightCommand); new JoystickButton(driverXbox, 2).whileTrue(driveToPoseCommand); new JoystickButton(driverXbox, XboxController.Button.kY.value).whileTrue(rotateCommand); -// new JoystickButton(driverXbox, 3).whileTrue(new RepeatCommand(new InstantCommand(drivebase::lock, drivebase))); - //new JoystickButton(driverXbox, XboxController.Button.kStart.value).whileTrue(drivebase.sysIdAngleMotorCommand()); - //new JoystickButton(driverXbox, XboxController.Button.kBack.value).whileTrue(drivebase.sysIdDriveMotorCommand()); new JoystickButton(shooterXbox, XboxController.Button.kLeftBumper.value).whileTrue(intakeCommand); // independent of speed diff --git a/src/main/java/frc/robot/commands/drivebase/AimAtLimelightCommand.java b/src/main/java/frc/robot/commands/drivebase/AimAtLimelightCommand.java index cec75c4..ec52542 100644 --- a/src/main/java/frc/robot/commands/drivebase/AimAtLimelightCommand.java +++ b/src/main/java/frc/robot/commands/drivebase/AimAtLimelightCommand.java @@ -42,10 +42,6 @@ public void initialize() { } - /** - * The main body of a command. Called repeatedly while the command is scheduled. - * (That is, it is called repeatedly until {@link #isFinished()}) returns true.) - */ @Override public void execute() { @@ -59,33 +55,11 @@ public void execute() { } - /** - *

- * Returns whether this command has finished. Once a command finishes -- indicated by - * this method returning true -- the scheduler will call its {@link #end(boolean)} method. - *

- * Returning false will result in the command never ending automatically. It may still be - * cancelled manually or interrupted by another command. Hard coding this command to always - * return true will result in the command executing once and finishing immediately. It is - * recommended to use * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} - * for such an operation. - *

- * - * @return whether this command has finished. - */ @Override public boolean isFinished() { return (Math.abs(LimelightHelpers.getTX("limelight-back")) < 1.0) && LimelightHelpers.getTV("limelight-back"); } - /** - * The action to take when the command ends. Called when either the command - * finishes normally -- that is it is called when {@link #isFinished()} returns - * true -- or when it is interrupted/canceled. This is where you may want to - * wrap up loose ends, like shutting off a motor that was being used in the command. - * - * @param interrupted whether the command was interrupted/canceled - */ @Override public void end(boolean interrupted) { LimelightHelpers.setPipelineIndex("limelight-back", 0); diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 98978ca..d73db77 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -43,7 +43,7 @@ private VisionSubsystem() { @Override public void periodic() { // This method will be called once per scheduler run - //SwerveSubsystem.addVisionMeasurement(LimelightHelpers.getBotPose2d("back-limelight"), Timer.getFPGATimestamp() - LimelightHelpers.getLatency_Capture("back-limelight") - LimelightHelpers.getLatency_Pipeline("back-limelight") ); + Logger.recordOutput("Targets", LimelightHelpers.getTargetPose3d_RobotSpace("limelight-back"));