diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d459c99..66eedc0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -164,6 +164,13 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); // ? /* Operator Buttons */ + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision)); + + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); + // creates an empty command & requires the swerve drive, subsystems can run only 1 command at a time // ? /* Programer Buttons (Controller 3)*/ diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java index 42a42c2..edfb97f 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -46,11 +46,17 @@ public class GotoPositionCommand extends Command { yPID.initialize(); } + double xerr; + double yerr; + double roterr; + double xytolerance = 0.01; + double rottolerance = 1; + @Override public void execute() { - double xerr = targetpos.getX() - vision.getPose2d().getX(); - double yerr = targetpos.getY() - vision.getPose2d().getY(); - double roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); + xerr = targetpos.getX() - vision.getPose2d().getX(); + yerr = targetpos.getY() - vision.getPose2d().getY(); + roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); SmartDashboard.putNumber("PID X Error", xerr); SmartDashboard.putNumber("PID Y Error", yerr); @@ -76,6 +82,13 @@ public class GotoPositionCommand extends Command { swerveDrive.driveWithInput(leftStick, rightStick, true); } + + @Override + public final boolean isFinished() { + return (Math.abs(xerr) < xytolerance && Math.abs(yerr) < xytolerance && Math.abs(roterr) < rottolerance); + // this statement is a boolean in and of itself + } + // @Override // public void end(boolean interrupted) {