diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java index c7e0149..bef973b 100644 --- a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java +++ b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java @@ -9,7 +9,6 @@ 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 { @@ -50,10 +49,7 @@ public class RotateUntilTarget extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - - try { this.visionOdometry.getTargetPoints(); } catch (VisionObscuredException voe) { return false; } - - return true; + return this.visionOdometry.getTargetPoints() == null; // return this.visionOdometry.m_camera.getLatestResult().hasTargets(); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 94f9bc8..f7a67b7 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -6,6 +6,7 @@ package frc4388.robot.commands.ShooterCommands; import java.util.ArrayList; import java.util.HashMap; +import java.util.logging.Logger; import java.util.stream.Collector; import java.util.stream.Collectors; @@ -28,6 +29,7 @@ import frc4388.utility.Vector2D; import frc4388.utility.desmos.DesmosServer; public class TrackTarget extends CommandBase { + private static final Logger LOGGER = Logger.getLogger(TrackTarget.class.getSimpleName()); /** Creates a new TrackTarget. */ SwerveDrive m_swerve; Turret m_turret; @@ -88,51 +90,50 @@ public class TrackTarget extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - try { - points = m_visionOdometry.getTargetPoints(); - // points = getFakePoints(); - //// points = filterPoints(points); - Point average = VisionOdometry.averagePoint(points); - - double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; - output *= 2.1; - - m_turret.runTurretWithInput(output); - // double position = m_turret.m_boomBoomRotateEncoder.getPosition(); - - // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || - // Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) - // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); - // else - // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), - // RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), - // true); - - - double regressedDistance = getDistance(average.y); - - // ! no longer a +30 lol -aarav - double distAdj = SmartDashboard.getNumber("Distance Adjust", -35); - velocity = m_boomBoom.getVelocity(regressedDistance + distAdj); - hoodPosition = m_boomBoom.getHood(regressedDistance + distAdj); - - m_boomBoom.runDrumShooterVelocityPID(velocity); - m_hood.runAngleAdjustPID(hoodPosition); - - double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity(); - double currentHood = this.m_hood.getEncoderPosition(); - - targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance) && (output < 0.2); - - - SmartDashboard.putNumber("Distance", regressedDistance - 35); - SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition); - SmartDashboard.putNumber("Vel Target Track", velocity); - SmartDashboard.putBoolean("Target Locked", targetLocked); - } catch (Exception e){ - e.printStackTrace(); + points = m_visionOdometry.getTargetPoints(); + if (points.isEmpty()) { + LOGGER.severe("Vision Obscured"); + return; } + // points = getFakePoints(); + //// points = filterPoints(points); + Point average = VisionOdometry.averagePoint(points); + double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + output *= 2.1; + + m_turret.runTurretWithInput(output); + // double position = m_turret.m_boomBoomRotateEncoder.getPosition(); + + // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || + // Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) + // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); + // else + // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), + // RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), + // true); + + + double regressedDistance = getDistance(average.y); + + // ! no longer a +30 lol -aarav + double distAdj = SmartDashboard.getNumber("Distance Adjust", -35); + velocity = m_boomBoom.getVelocity(regressedDistance + distAdj); + hoodPosition = m_boomBoom.getHood(regressedDistance + distAdj); + + m_boomBoom.runDrumShooterVelocityPID(velocity); + m_hood.runAngleAdjustPID(hoodPosition); + + double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity(); + double currentHood = this.m_hood.getEncoderPosition(); + + targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance) && (output < 0.2); + + + SmartDashboard.putNumber("Distance", regressedDistance - 35); + SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition); + SmartDashboard.putNumber("Vel Target Track", velocity); + SmartDashboard.putBoolean("Target Locked", targetLocked); } public ArrayList filterPoints(ArrayList points) { diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 49a2a61..fda2586 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -24,7 +24,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.VisionConstants; -import frc4388.utility.VisionObscuredException; /** Represents the limelight and odometry related functionality * @author Daniel McGrath @@ -61,16 +60,15 @@ public class VisionOdometry extends SubsystemBase { * @return Vision points on the rim of the target in screen space * @throws VisionObscuredException */ - public ArrayList getTargetPoints() throws VisionObscuredException { + public ArrayList getTargetPoints() { PhotonPipelineResult result = m_camera.getLatestResult(); latency = result.getLatencyMillis(); System.out.println("Result: " + result.hasTargets() + ", latency: " + latency); - - if(!result.hasTargets()) - throw new VisionObscuredException(); - + ArrayList points = new ArrayList<>(); + if(!result.hasTargets()) + return points; for(PhotonTrackedTarget target : result.getTargets()) { List corners = target.getCorners(); @@ -102,11 +100,11 @@ public class VisionOdometry extends SubsystemBase { m_camera.setDriverMode(driverMode); } - public Point getTargetOffset() throws VisionObscuredException { + public Point getTargetOffset() { ArrayList screenPoints = getTargetPoints(); if(screenPoints.size() < 3) - throw new VisionObscuredException("Less than 3 vision points available"); + return null; ArrayList points3d = get3dPoints(screenPoints); ArrayList points = topView(points3d); @@ -125,8 +123,10 @@ public class VisionOdometry extends SubsystemBase { * @return The estimated odometry pose, including gyro rotation * @throws VisionObscuredException */ - public Pose2d getVisionOdometry() throws VisionObscuredException { + public Pose2d getVisionOdometry() { Point targetOffset = getTargetOffset(); + if (targetOffset == null) + return null; targetOffset = correctGuessForCenter(targetOffset, m_shooter.getBoomBoomAngleDegrees()); targetOffset = correctGuessForGyro(targetOffset, m_drive.getRegGyro().getDegrees()); diff --git a/src/main/java/frc4388/utility/VisionObscuredException.java b/src/main/java/frc4388/utility/VisionObscuredException.java deleted file mode 100644 index cc33066..0000000 --- a/src/main/java/frc4388/utility/VisionObscuredException.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc4388.utility; - -/** Exception that occurs if the limelight can't see enough points - * @author Daniel Thomas McGrath - */ -public class VisionObscuredException extends Exception { - /** - * Creates new VisionObscuredException with error text 'null' - */ - public VisionObscuredException() { - super("Unable to see sufficient vision points"); - } - - /** Creates new VisionObscuredException with error text message - * - * @param message Error text message - */ - public VisionObscuredException(String message) { - super(message); - } - - /** Creates new VisionObscuredException with error text message and detailed stack trace - * - * @param message Error text message - * @param cause Root cause of error - */ - public VisionObscuredException(String message, Throwable cause) { - super(message, cause); - } - - /** Creates new VisionObscuredException with error text 'null' and detailed stack trace - * - * @param cause Root cause of error - */ - public VisionObscuredException(Throwable cause) { - super("Unable to see sufficient vision points", cause); - } -}