diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6ef3fbb2..78acf9a3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -195,6 +195,16 @@ public enum AccelerationLimits { */ public static final double MAX_ANGULAR_ACCELERATION = Math.toRadians(360 * 22); + /** + * Angular offset of the intake limelight (degrees) + */ + public static final double INTAKE_LIMELIGHT_ANGLE_OFFSET = 50; + /** + * Height offset of the intake limelight from the ground (inches) + */ + public static final double INTAKE_LIMELIGHT_HEIGHT_OFFSET = 0.75; + public static final double MAX_CENTERING_SPEED = 0.5; + //field/Vision Manager constants public static final Translation2d GOAL_POSITION = new Translation2d(8.25, 0); public static final double VISION_PREDICT_AHEAD_TIME = 0.5; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e2457cc2..3e72fe02 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,7 +20,9 @@ import frc.subsystem.*; import frc.subsystem.Climber.ClimbState; import frc.subsystem.Hopper.HopperState; +import frc.subsystem.Intake.IntakeSolState; import frc.utility.*; +import frc.utility.Controller.XboxAxes; import frc.utility.Controller.XboxButtons; import frc.utility.Limelight.LedMode; import frc.utility.Limelight.StreamingMode; @@ -525,7 +527,6 @@ public void teleopInit() { } private final Object driverForcingVisionOn = new Object(); - private final Object buttonPanelForcingVisionOn = new Object(); private final Object resettingPoseVisionOn = new Object(); /* @@ -546,8 +547,7 @@ public void teleopPeriodic() { // Will terminate climb auto thread if any stick movement happens - if (autoThread != null && autoThread.isAlive() && - !getControllerDriveInputs().equals(NO_MOTION_CONTROLLER_INPUTS)) { + if (autoThread != null && autoThread.isAlive() && !getControllerDriveInputs().equals(NO_MOTION_CONTROLLER_INPUTS)) { killAuto(); } @@ -607,12 +607,18 @@ public void teleopPeriodic() { } visionManager.unForceVisionOn(driverForcingVisionOn); + + if (GRAPPLE_CLIMB || Climber.getInstance().getClimbState() == ClimbState.IDLE || Climber.getInstance().isPaused()) { // If we're climbing don't allow the robot to be driven if (usingDPad) { drive.updateTurn(getControllerDriveInputs(), Constants.CLIMB_LINEUP_ANGLE, useFieldRelative, 0); } else { - doNormalDriving(); + if (xbox.getRawAxis(XboxAxes.LEFT_TRIGGER) > 0.1 && intake.getIntakeSolState() == IntakeSolState.OPEN) { + drive.centerOntoBall(getControllerDriveInputs(), useFieldRelative); + } else { + doNormalDriving(); + } } } else { // Stop the robot from moving if we're not issuing other commands to the drivebase @@ -870,7 +876,7 @@ public void testPeriodic() { final Hopper hopper = Hopper.getInstance(); final Intake intake = Intake.getInstance(); final Shooter shooter = Shooter.getInstance(); - + if (!Constants.GRAPPLE_CLIMB) { Climber climber = Climber.getInstance(); diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index fb9d134f..ab5210d4 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; import frc.utility.ControllerDriveInputs; +import frc.utility.Limelight; import frc.utility.Timer; import frc.utility.controllers.LazyTalonFX; import frc.utility.wpimodified.HolonomicDriveController; @@ -76,7 +77,7 @@ public void resetAuto() { } public enum DriveState { - TELEOP, TURN, HOLD, DONE, RAMSETE, STOP + TELEOP, TURN, HOLD, DONE, RAMSETE, SEARCH_FOR_BALL, STOP } public boolean useRelativeEncoderPosition = false; @@ -678,6 +679,67 @@ private void updateRamsete() { } } + PIDController centerOntoBallPID = new PIDController(1, 0, 0, 0.02); + + public void centerOntoBall(ControllerDriveInputs inputs, boolean fieldRelativeEnabled) { + Limelight limelight = Limelight.getInstance("limelight-intake"); + if (limelight.isTargetVisible()) { + + /* Top Down View + [] - ball + X + []------ + \ | + \ | + \ | Y + \ | + \ | + \| + |---|----|----|----|----| Intake + */ + + double distY = Math.tan(Math.toRadians(limelight.getVerticalOffset() + INTAKE_LIMELIGHT_ANGLE_OFFSET)) + * INTAKE_LIMELIGHT_HEIGHT_OFFSET; + + double distX = Math.tan(limelight.getHorizontalOffset()) * distY; + + + double allowedError = DriverStation.isAutonomous() ? 0 : (distY * 0.2) + 20; + + double pidError; + if (Math.abs(distX) < allowedError) { + pidError = 0; + } else { + pidError = distX - (Math.signum(distX) * allowedError); + } + + double strafeCommand = centerOntoBallPID.calculate(pidError, 0); + strafeCommand = Math.signum(strafeCommand) * Math.min(Math.abs(strafeCommand), MAX_CENTERING_SPEED); + // Ensure that the driver can always override the PID + + Translation2d correction = new Translation2d(0, strafeCommand); + + ChassisSpeeds chassisSpeeds; + if (useFieldRelative && fieldRelativeEnabled) { + correction = correction.rotateBy(RobotTracker.getInstance().getGyroAngle()); //Make it perpendicular to the robot + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + DRIVE_HIGH_SPEED_M * inputs.getX() + correction.getX(), + DRIVE_HIGH_SPEED_M * inputs.getY() + correction.getY(), + inputs.getRotation() * 7, + RobotTracker.getInstance().getGyroAngle()); + } else { + chassisSpeeds = new ChassisSpeeds( + DRIVE_HIGH_SPEED_M * inputs.getX() + correction.getX(), + DRIVE_HIGH_SPEED_M * inputs.getY() + correction.getY(), + inputs.getRotation() * 7); + } + + swerveDrive(chassisSpeeds); + } else { + swerveDriveFieldRelative(inputs); + } + } + private double getTurnPidDeltaSpeed(@NotNull TrapezoidProfile.State autoAimingRotationGoal) { turnPID.setSetpoint(autoAimingRotationGoal.position); @@ -725,6 +787,20 @@ public void update() { break; case STOP: swerveDrive(new ChassisSpeeds(0, 0, 0)); + break; + case SEARCH_FOR_BALL: + searchForBall(); + break; + } + } + + private void searchForBall() { + Limelight intakeLimelight = Limelight.getInstance("limelight-intake"); + if (intakeLimelight.isTargetVisible()) { + Translation2d movement = new Translation2d(0.25, 0); + centerOntoBall(new ControllerDriveInputs(movement.getX(), movement.getY(), 0), false); + } else { + swerveDrive(new ChassisSpeeds(0, 0, 0)); } } @@ -859,6 +935,15 @@ public void logData() { logData("Swerve Motor " + i + " Current", swerveMotors[i].getStatorCurrent()); } logData("Drive State", driveState.toString()); + + Limelight limelight = Limelight.getInstance("limelight-intake"); + if (limelight.isTargetVisible()) { + double distY = Math.tan(Math.toRadians(limelight.getVerticalOffset() + INTAKE_LIMELIGHT_ANGLE_OFFSET)) + * INTAKE_LIMELIGHT_HEIGHT_OFFSET; + double distX = Math.tan(limelight.getHorizontalOffset()) * distY; + logData("Limelight Distance X", distX); + logData("Limelight Distance Y", distY); + } } @@ -946,4 +1031,8 @@ public void turnToAngle(double degrees) throws InterruptedException { Thread.sleep(20); } } + + public synchronized void setSearchForBall() { + setDriveState(DriveState.SEARCH_FOR_BALL); + } } diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 90404d26..5b794d9e 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.drive.Vector2d; import frc.robot.Constants; import frc.subsystem.BlinkinLED.BlinkinLedMode; import frc.subsystem.BlinkinLED.LedStatus; @@ -86,17 +85,8 @@ public void logData() { logData("Distance to Target", Units.metersToInches(getDistanceToTarget())); logData("Rotation Target", getAngleToTarget().getDegrees()); - - Vector3D correctVector = limelight.getCorrectTargetVector(); - logData("New Distance", Math.hypot(correctVector.getX(), correctVector.getZ())); - logData("Old Distance ", limelight.getDistance() + Constants.GOAL_RADIUS_IN + 23); - Vector2d targetPx = limelight.getTargetPosInCameraPixels(); - - logData("py", targetPx.y); - logData("px", targetPx.x); - // Vector2d newRelGoalPos = limelight.getCorrectGoalPos(); // logData("New Z", newRelGoalPos.x); // logData("New X", newRelGoalPos.y); diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java index b5cba141..72ea5421 100644 --- a/src/main/java/frc/utility/Limelight.java +++ b/src/main/java/frc/utility/Limelight.java @@ -340,6 +340,7 @@ public Vector2d getTargetPosInCameraPixels() { ); private static final MatBuilder THREE_BY_ONE_MAT_BUILDER = new MatBuilder<>(Nat.N3(), Nat.N1()); + /** * @return The Correct distance from the limelight to the target in IN. This correctly compensates for the angle of the camera * to ensure a consistent distance as the robot rotates.