diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 94da529..910b44f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -292,7 +292,7 @@ public class RobotContainer { /* Operator Buttons */ new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)); + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java index efcbaa3..310b77e 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup { public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood, false), new TrackTarget(turret, drum, hood, visionOdometry)); + addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry)); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index ecd88ab..f3d1920 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -17,6 +17,7 @@ import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; import frc4388.utility.DummySensor; import frc4388.utility.Gains; @@ -27,6 +28,7 @@ public class Shoot extends CommandBase { private Turret turret; private BoomBoom drum; private Hood hood; + private VisionOdometry visionOdometry; private boolean toShoot; @@ -56,6 +58,8 @@ public class Shoot extends CommandBase { private int inverted; private double initialSwerveRotation; + private boolean endsWithLimelight; + /** * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball * @@ -66,7 +70,7 @@ public class Shoot extends CommandBase { * * @author Aarav Shah */ - public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, boolean toShoot) { + public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, boolean toShoot, boolean endsWithLimelight) { // Use addRequirements() here to declare subsystem dependencies. this.swerve = swerve; this.drum = drum; @@ -74,6 +78,7 @@ public class Shoot extends CommandBase { this.hood = hood; this.toShoot = toShoot; + this.endsWithLimelight = endsWithLimelight; kP = turretGains.kP; kI = turretGains.kI; @@ -89,7 +94,7 @@ public class Shoot extends CommandBase { this.inverted = 1; - addRequirements(this.swerve, this.drum, this.turret, this.hood); + addRequirements(this.swerve, this.drum, this.turret, this.hood, this.visionOdometry); } // public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { @@ -194,12 +199,15 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - - if (isAimedInTolerance && !timerStarted) { - timerStarted = true; - startTime = System.currentTimeMillis(); + if (!endsWithLimelight) { + if (isAimedInTolerance && !timerStarted) { + timerStarted = true; + startTime = System.currentTimeMillis(); + } + SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + } else { + return this.visionOdometry.m_camera.getLatestResult().hasTargets(); } - SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); - return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); } } diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 2a0abd2..315623d 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -31,7 +31,7 @@ import frc4388.utility.VisionObscuredException; */ public class VisionOdometry extends SubsystemBase { // roborio ip & port: 10.43.88.2:1735 - private PhotonCamera m_camera; + public PhotonCamera m_camera; private SwerveDrive m_drive; private Turret m_shooter;