From 96f2dfec2b252a57a72e340cd81723346ce8485a Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 08:51:49 -0700 Subject: [PATCH 1/9] cube/square the vector length of the controller inputs, not the individual components --- .../java/frc/utility/ControllerDriveInputs.java | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/utility/ControllerDriveInputs.java b/src/main/java/frc/utility/ControllerDriveInputs.java index ae184515..aa8577bf 100644 --- a/src/main/java/frc/utility/ControllerDriveInputs.java +++ b/src/main/java/frc/utility/ControllerDriveInputs.java @@ -74,8 +74,13 @@ public double getRotation() { * @return {@link ControllerDriveInputs} */ public @NotNull ControllerDriveInputs squareInputs() { - x = Math.copySign(x * x, x); - y = Math.copySign(y * y, y); + double dist = Math.hypot(x, y); + double distSquared = dist * dist; + double angle = Math.atan2(y, x); + + + x = distSquared * Math.cos(angle); + y = distSquared * Math.sin(angle); rotation = Math.copySign(rotation * rotation, rotation); return this; } @@ -86,8 +91,12 @@ public double getRotation() { * @return {@link ControllerDriveInputs} */ public @NotNull ControllerDriveInputs cubeInputs() { - x = x * x * x; - y = y * y * y; + double dist = Math.hypot(x, y); + double distCubed = dist * dist * dist; + double angle = Math.atan2(y, x); + + x = distCubed * Math.cos(angle); + y = distCubed * Math.sin(angle); rotation = rotation * rotation * rotation; return this; } From 7b7e336b1111592dc07f88766201b59117e6b8ad Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 09:13:59 -0700 Subject: [PATCH 2/9] add code to strafe the robot so that it centers with balls --- src/main/java/frc/subsystem/Drive.java | 64 +++++++++++++++++++++++++- 1 file changed, 63 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 92d713a2..f930b37c 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -28,6 +28,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; @@ -65,7 +66,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; @@ -573,6 +574,53 @@ private void updateRamsete() { } } + PIDController centerOntoBallPID = new PIDController(1, 0, 0, 0.02); + + private void centerOntoBall(ControllerDriveInputs inputs) { + Limelight limelight = Limelight.getInstance("limelight-intake"); + if (limelight.isTargetVisible()) { + + /* Top Down View + [] - ball + X + []------ + \ | + \ | + \ | Y + \ | + \ | + \| + |---|----|----|----|----| Intake + */ + + double distY = Math.tan(Math.toRadians(limelight.getVerticalOffset() + 50)) * 20; + + double distX = Math.tan(limelight.getHorizontalOffset()) * distY; + + double allowedError = (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); + Translation2d correction = new Translation2d(strafeCommand, 0).rotateBy( + RobotTracker.getInstance().getGyroAngle()); //Make it perpendicular to the robot + + ChassisSpeeds 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()); + swerveDrive(chassisSpeeds); + } else { + swerveDriveFieldRelative(inputs); + } + } + public void setAutoRotation(@NotNull Rotation2d rotation) { autoTargetHeading = rotation; System.out.println("new rotation" + rotation.getDegrees()); @@ -603,6 +651,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).rotateBy(RobotTracker.getInstance().getGyroAngle()); + centerOntoBall(new ControllerDriveInputs(movement.getX(), movement.getY(), 0)); + } else { + swerveDrive(new ChassisSpeeds(0, 0, 0)); } } From 745cada76375f96a63a0365f60f106c5d99e56b9 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 09:37:07 -0700 Subject: [PATCH 3/9] update the shooter config --- src/main/deploy/shooter/shooterconfig.json | 34 +++++++++++----------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json index 9e0ddcfe..c1d58560 100644 --- a/src/main/deploy/shooter/shooterconfig.json +++ b/src/main/deploy/shooter/shooterconfig.json @@ -1,51 +1,51 @@ { "shooterConfigs" : [ { - "hoodEjectAngle" : 67.0, - "flywheelSpeed" : 1800.0, + "hoodEjectAngle" : 68.0, + "flywheelSpeed" : 1700.0, "distance" : 40.0 }, { "hoodEjectAngle" : 62.0, - "flywheelSpeed" : 1750.0, + "flywheelSpeed" : 1700.0, "distance" : 60.0 }, { "hoodEjectAngle" : 56.0, - "flywheelSpeed" : 1800.0, + "flywheelSpeed" : 1700.0, "distance" : 82.0 }, { "hoodEjectAngle" : 56.0, - "flywheelSpeed" : 2000.0, + "flywheelSpeed" : 1800.0, "distance" : 106.0 }, { "hoodEjectAngle" : 53.0, - "flywheelSpeed" : 2000.0, + "flywheelSpeed" : 1800.0, "distance" : 115.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2000.0, + "flywheelSpeed" : 1900.0, "distance" : 120.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2100.0, + "flywheelSpeed" : 2000.0, "distance" : 133.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2150.0, + "flywheelSpeed" : 2100.0, "distance" : 144.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2300.0, + "flywheelSpeed" : 2200.0, "distance" : 157.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2350.0, + "flywheelSpeed" : 2200.0, "distance" : 175.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2460.0, + "flywheelSpeed" : 2300.0, "distance" : 190.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2450.0, + "flywheelSpeed" : 2400.0, "distance" : 200.0 }, { "hoodEjectAngle" : 51.0, @@ -53,19 +53,19 @@ "distance" : 210.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2550.0, + "flywheelSpeed" : 2600.0, "distance" : 225.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 2650.0, + "flywheelSpeed" : 2700.0, "distance" : 250.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 3000.0, + "flywheelSpeed" : 2800.0, "distance" : 270.0 }, { "hoodEjectAngle" : 51.0, - "flywheelSpeed" : 3000.0, + "flywheelSpeed" : 2900.0, "distance" : 299.0 } ] } \ No newline at end of file From b7246327af34755eb6957ecf8e601200488d9f66 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 10:08:47 -0700 Subject: [PATCH 4/9] add method to run searchForBall in autonomous --- src/main/java/frc/subsystem/Drive.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index f930b37c..4c13d245 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -898,4 +898,8 @@ public void turnToAngle(double degrees) throws InterruptedException { Thread.onSpinWait(); } } + + public synchronized void setSearchForBall() { + setDriveState(DriveState.SEARCH_FOR_BALL); + } } From 1853a1337959b763fd0e4ae2057e7ca62ef7df47 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 10:21:57 -0700 Subject: [PATCH 5/9] make centeringOnBall work in teleop when you turn on the intake --- src/main/java/frc/robot/Robot.java | 6 ++++-- src/main/java/frc/subsystem/Drive.java | 29 +++++++++++++++++--------- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6e912907..49925f9b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,6 +20,7 @@ import frc.subsystem.*; import frc.subsystem.Climber.ClimbState; import frc.utility.*; +import frc.utility.Controller.XboxAxes; import frc.utility.Controller.XboxButtons; import frc.utility.Limelight.LedMode; import frc.utility.Limelight.StreamingMode; @@ -436,7 +437,6 @@ public void teleopInit() { } private final Object driverForcingVisionOn = new Object(); - private final Object buttonPanelForcingVisionOn = new Object(); private final Object resettingPoseVisionOn = new Object(); /** @@ -464,7 +464,9 @@ public void teleopPeriodic() { shooter.setFiring(false); shooter.setSpeed(0); visionManager.unForceVisionOn(driverForcingVisionOn); - if (climber.getClimbState() == ClimbState.IDLE || climber.isPaused()) { // If we're climbing don't allow the robot to be + if (xbox.getRawAxis(XboxAxes.LEFT_TRIGGER) > 0.1) { + drive.centerOntoBall(getControllerDriveInputs(), useFieldRelative); + } else if (climber.getClimbState() == ClimbState.IDLE || climber.isPaused()) { // If we're climbing don't allow the robot to be // driven doNormalDriving(); } else { diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 4c13d245..3ef8fafc 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -576,7 +576,7 @@ private void updateRamsete() { PIDController centerOntoBallPID = new PIDController(1, 0, 0, 0.02); - private void centerOntoBall(ControllerDriveInputs inputs) { + public void centerOntoBall(ControllerDriveInputs inputs, boolean fieldRelativeEnabled) { Limelight limelight = Limelight.getInstance("limelight-intake"); if (limelight.isTargetVisible()) { @@ -607,14 +607,23 @@ private void centerOntoBall(ControllerDriveInputs inputs) { } double strafeCommand = centerOntoBallPID.calculate(pidError, 0); - Translation2d correction = new Translation2d(strafeCommand, 0).rotateBy( - RobotTracker.getInstance().getGyroAngle()); //Make it perpendicular to the robot + 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); + } - ChassisSpeeds 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()); swerveDrive(chassisSpeeds); } else { swerveDriveFieldRelative(inputs); @@ -661,8 +670,8 @@ public void update() { private void searchForBall() { Limelight intakeLimelight = Limelight.getInstance("limelight-intake"); if (intakeLimelight.isTargetVisible()) { - Translation2d movement = new Translation2d(0.25, 0).rotateBy(RobotTracker.getInstance().getGyroAngle()); - centerOntoBall(new ControllerDriveInputs(movement.getX(), movement.getY(), 0)); + Translation2d movement = new Translation2d(0.25, 0); + centerOntoBall(new ControllerDriveInputs(movement.getX(), movement.getY(), 0), false); } else { swerveDrive(new ChassisSpeeds(0, 0, 0)); } From a070d2ab7e2e493195688701c40bbb388284d63c Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 10:47:49 -0700 Subject: [PATCH 6/9] only assist the driver in lining with balls if the intake is open --- src/main/java/frc/robot/Robot.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 49925f9b..a7f0d95d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,6 +19,7 @@ import frc.auton.guiauto.serialization.reflection.ClassInformationSender; import frc.subsystem.*; import frc.subsystem.Climber.ClimbState; +import frc.subsystem.Intake.IntakeSolState; import frc.utility.*; import frc.utility.Controller.XboxAxes; import frc.utility.Controller.XboxButtons; @@ -464,7 +465,7 @@ public void teleopPeriodic() { shooter.setFiring(false); shooter.setSpeed(0); visionManager.unForceVisionOn(driverForcingVisionOn); - if (xbox.getRawAxis(XboxAxes.LEFT_TRIGGER) > 0.1) { + if (xbox.getRawAxis(XboxAxes.LEFT_TRIGGER) > 0.1 && intake.getIntakeSolState() == IntakeSolState.OPEN) { drive.centerOntoBall(getControllerDriveInputs(), useFieldRelative); } else if (climber.getClimbState() == ClimbState.IDLE || climber.isPaused()) { // If we're climbing don't allow the robot to be // driven From bb581033035ec95c121b347e7605e5749749e24b Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 24 Mar 2022 22:17:58 -0700 Subject: [PATCH 7/9] change distance aprox for intake cam to use constants camp the max output for the centering pid to be 0.5 --- src/main/java/frc/robot/Constants.java | 10 ++++++++++ src/main/java/frc/subsystem/Drive.java | 6 +++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index db15f03b..511631b9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -180,6 +180,16 @@ public final class Constants { */ public static final double MAX_ANGULAR_ACCELERATION = Math.toRadians(360 * 27); + /** + * 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/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 3ef8fafc..73d37fa5 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -593,7 +593,8 @@ public void centerOntoBall(ControllerDriveInputs inputs, boolean fieldRelativeEn |---|----|----|----|----| Intake */ - double distY = Math.tan(Math.toRadians(limelight.getVerticalOffset() + 50)) * 20; + double distY = Math.tan(Math.toRadians(limelight.getVerticalOffset() + INTAKE_LIMELIGHT_ANGLE_OFFSET)) + * INTAKE_LIMELIGHT_HEIGHT_OFFSET; double distX = Math.tan(limelight.getHorizontalOffset()) * distY; @@ -607,6 +608,9 @@ public void centerOntoBall(ControllerDriveInputs inputs, boolean fieldRelativeEn } 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; From 26dd0c4677311c3a89e920303fe64d1c004ee416 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 25 Mar 2022 14:36:33 -0700 Subject: [PATCH 8/9] center the robot with the ball when running autos also log the calculated distances to the ball to shuffleboard --- src/main/java/frc/subsystem/Drive.java | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 73d37fa5..159b81e8 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -598,7 +598,8 @@ public void centerOntoBall(ControllerDriveInputs inputs, boolean fieldRelativeEn double distX = Math.tan(limelight.getHorizontalOffset()) * distY; - double allowedError = (distY * 0.2) + 20; + + double allowedError = DriverStation.isAutonomous() ? 0 : (distY * 0.2) + 20; double pidError; if (Math.abs(distX) < allowedError) { @@ -824,6 +825,15 @@ public void logData() { } 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); + } } From 15605af03edfff44656caef2aa7071a6557285db Mon Sep 17 00:00:00 2001 From: varun7654 Date: Fri, 25 Mar 2022 14:37:14 -0700 Subject: [PATCH 9/9] detect if the vision target is touching the edge of the screen. If it is we will not use that vision data. --- .../java/frc/subsystem/VisionManager.java | 5 ++-- src/main/java/frc/utility/Limelight.java | 24 +++++++++++++++++++ 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 99d96ac1..020d3035 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -328,10 +328,9 @@ public void update() { logData("Vision Pose Angle", visionPose.getRotation().getRadians()); logData("Vision Pose Time", getLimelightTime()); - //TODO: Check that the contours aren't touching the edge of the screen before using them if (MathUtil.dist2(robotTracker.getLatencyCompedPoseMeters().getTranslation().plus(robotPositionOffset), - robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED) { - + robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED + && !limelight.areCornersTouchingEdge()) { robotTracker.addVisionMeasurement(robotTranslation, getLimelightTime()); diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java index 7c84d4be..e285f88a 100644 --- a/src/main/java/frc/utility/Limelight.java +++ b/src/main/java/frc/utility/Limelight.java @@ -5,6 +5,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.drive.Vector2d; import org.jetbrains.annotations.NotNull; import java.util.HashMap; @@ -14,6 +15,7 @@ * This class is used to get data from the limelight network tables */ public final class Limelight { + public static final double[] EMPTY_DOUBLE_ARRAY = new double[0]; final @NotNull NetworkTable limelightTable; final @NotNull NetworkTable limelightGuiTable; @@ -260,4 +262,26 @@ public double getDistance() { return 0; } } + + + public Vector2d[] getCorners() { + Vector2d[] processedCorners = new Vector2d[4]; + double[] corners = limelightTable.getEntry("tcornxy").getDoubleArray(EMPTY_DOUBLE_ARRAY); + for (int i = 0; i < corners.length; i += 2) { + if (i + 1 < corners.length) { + processedCorners[i / 2] = new Vector2d(corners[i], corners[i + 1]); + } + } + return processedCorners; + } + + public boolean areCornersTouchingEdge() { + Vector2d[] corners = getCorners(); + for (Vector2d corner : corners) { + if (corner.x < 2 || corner.x > 318 || corner.y < 2 || corner.y > 238) { + return true; + } + } + return false; + } } \ No newline at end of file