diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 55bbf1f..c6deaeb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -451,10 +451,16 @@ public class RobotContainer { // new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive), // "Diamond")); + // * assume turret is already pointed towards target. return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0), - new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5), new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + + // * aim with RotateUntilTarget + // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), + // new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0), + // new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5), + // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); } public static XboxController getDriverController() { diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java index 76ec283..c7e0149 100644 --- a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java +++ b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java @@ -4,9 +4,12 @@ package frc4388.robot.commands.DriveCommands; +import javax.swing.plaf.basic.BasicTreeUI.TreeCancelEditingAction; + import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.VisionObscuredException; public class RotateUntilTarget extends CommandBase { @@ -47,6 +50,10 @@ public class RotateUntilTarget extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return this.visionOdometry.m_camera.getLatestResult().hasTargets(); + + try { this.visionOdometry.getTargetPoints(); } catch (VisionObscuredException voe) { return false; } + + return true; + // return this.visionOdometry.m_camera.getLatestResult().hasTargets(); } }