diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9600169..5ae5339 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,11 +175,25 @@ public final class Constants { } public static final class VisionConstants { - public static final double TURN_P_VALUE = 0.8; - public static final double X_ANGLE_ERROR = 0.5; - public static final double GRAV = 385.83; - public static final double TARGET_HEIGHT = 67.5; - public static final double FOV = 29.8; //Field of view limelight + // public static final double TURN_P_VALUE = 0.8; + // public static final double X_ANGLE_ERROR = 0.5; + // public static final double GRAV = 385.83; + // public static final double TARGET_HEIGHT = 67.5; + // public static final double FOV = 29.8; //Field of view limelight public static final double LIME_ANGLE = 24.7; + + public static final String NAME = "photonCamera"; + + public static final double TARGET_HEIGHT = 8*12 + 8; // Convert to metric + public static final double TARGET_RADIUS = 4*12; // Convert to metric + public static final double H_FOV = 59.6; + public static final double V_FOV = 49.7; + public static final double LIME_VIXELS = 960; + public static final double LIME_HIXELS = 720; + + public static final double RANGE = 10; + + public static final double LIMELIGHT_RADIUS = 1.d; + public static final double SHOOTER_CORRECTION = 1.d; } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dd05ec5..ef07b7e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -64,6 +64,7 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Vision; +import frc4388.robot.subsystems.VisionOdometry; import frc4388.utility.LEDPatterns; import frc4388.utility.ListeningSendableChooser; import frc4388.utility.PathPlannerUtil; @@ -88,8 +89,10 @@ public class RobotContainer { private final LED m_robotLED = new LED(m_robotMap.LEDController); private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); - // private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); // private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); + /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -119,7 +122,7 @@ public class RobotContainer { //Turret default command - // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive)); + m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> m_robotSwerveDrive.driveWithInput( diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 649fcb2..4f1478f 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -136,7 +136,7 @@ public class RobotMap { public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - // public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); // Create motor CANSparkMax void configureShooterMotorControllers() { diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 45f0998..144bb74 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -6,13 +6,16 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; public class AimToCenter extends CommandBase { /** Creates a new AimWithOdometry. */ Turret m_turret; SwerveDrive m_drive; + VisionOdometry m_visionOdometry; // use odometry to find x and y later double x; @@ -21,11 +24,12 @@ public class AimToCenter extends CommandBase { // public static Gains m_aimGains; - public AimToCenter(Turret turret, SwerveDrive drive) { + public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) { // Use addRequirements() here to declare subsystem dependencies. m_turret = turret; m_drive = drive; - addRequirements(m_turret, m_drive); + m_visionOdometry = visionOdometry; + addRequirements(m_turret, m_drive, m_visionOdometry); } // Called when the command is initially scheduled. @@ -40,6 +44,9 @@ public class AimToCenter extends CommandBase { public void execute() { m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); m_turret.runshooterRotatePID(m_targetAngle); + + // Check if limelight is within range + m_visionOdometry.setLEDs(Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE); } public static double angleToCenter(double x, double y, double gyro) { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e29404a..c32e833 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -17,12 +17,14 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +import frc4388.utility.VisionObscuredException; public class SwerveDrive extends SubsystemBase { @@ -51,6 +53,7 @@ public class SwerveDrive extends SubsystemBase { below are robot specific, and should be tuned. */ public SwerveDrivePoseEstimator m_poseEstimator; public SwerveDriveOdometry m_odometry; + public VisionOdometry m_visionOdometry; public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; @@ -227,11 +230,14 @@ public class SwerveDrive extends SubsystemBase { modules[2].getState(), modules[3].getState()); - // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on - // a real robot, this must be calculated based either on latency or timestamps. - // m_poseEstimator.addVisionMeasurement( - // m_poseEstimator.getEstimatedPosition(), - // Timer.getFPGATimestamp() - 0.1); + // Also apply vision measurements if the camera can get vision + try { + m_poseEstimator.addVisionMeasurement( + m_visionOdometry.getVisionOdometry(), + Timer.getFPGATimestamp() - m_visionOdometry.getLatency()); + } catch (VisionObscuredException err) { + err.printStackTrace(); + } } /** diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java new file mode 100644 index 0000000..29bdc18 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -0,0 +1,279 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import java.util.ArrayList; +import java.util.List; + +import org.opencv.core.Point; +import org.opencv.core.Point3; +import org.photonvision.PhotonCamera; +import org.photonvision.common.hardware.VisionLEDMode; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +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 + */ +public class VisionOdometry extends SubsystemBase { + // roborio ip & port: 10.43.88.2:1735 + private PhotonCamera m_camera; + + private SwerveDrive m_drive; + private Turret m_shooter; + + private double latency = 0; + + /** Creates a new VisionOdometry + * + * @param drive The swerve drive subsystem + * @param shooter The turret subsystem + */ + public VisionOdometry(SwerveDrive drive, Turret shooter) { + m_camera = new PhotonCamera(VisionConstants.NAME); + m_drive = drive; + m_shooter = shooter; + } + + /** Gets the vision points from the limelight + *

+ * Breaks down targets into 4 corners and uses the top 2 points + * + * @return Vision points on the rim of the target in screen space + */ + public ArrayList getTargetPoints() { + PhotonPipelineResult result = m_camera.getLatestResult(); + latency = result.getLatencyMillis(); + + if(!result.hasTargets()) + return new ArrayList(); + + ArrayList points = new ArrayList<>(); + + for(PhotonTrackedTarget target : result.getTargets()) { + List corners = target.getCorners(); + + double centerY = 0; + for(TargetCorner corner : corners) { + centerY += corner.y; + } + centerY /= corners.size(); + + for(TargetCorner corner : corners) { + if(corner.y <= centerY) + points.add(new Point(corner.x, VisionConstants.LIME_VIXELS - corner.y)); + } + } + + return points; + } + + /** Sets LEDs on or off (duh) + * + * @param on LED state + */ + public void setLEDs(boolean on) { + m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + /** Gets estimated odometry based on limelight data + * + * @return The estimated odometry pose, including gyro rotation + * @throws VisionObscuredException + */ + public Pose2d getVisionOdometry() throws VisionObscuredException { + ArrayList screenPoints = getTargetPoints(); + + if(screenPoints.size() < 3) + throw new VisionObscuredException("Not enough vision points available"); + + ArrayList points3d = get3dPoints(screenPoints); + ArrayList points = topView(points3d); + + Point guess = averagePoint(points); + + for(int i = 0; i < 30; i++) { + guess = iterateGuess(guess, points); + } + + guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees()); + guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees()); + + SmartDashboard.putNumber("Vision ODO x: ", guess.x); + SmartDashboard.putNumber("Vision ODO y: ", guess.y); + + Rotation2d rotation = new Rotation2d(Math.toDegrees(m_drive.getRegGyro().getDegrees())); + Pose2d odometryPose = new Pose2d(guess.x, guess.y, rotation); + + return odometryPose; + } + + public double getLatency() { + return latency; + } + + /** Reverse 3d projects target points from screen coordinates to 3d space + *

+ * Uses the known height of the target to project points + * + * @param points2d Vision points on the rim of the target in screen space + * @return Reverse 3d projected points + */ + public static final ArrayList get3dPoints(ArrayList points2d) { + ArrayList points3d = new ArrayList<>(); + + for(Point point2d : points2d) { + double y_rot = point2d.y / VisionConstants.LIME_VIXELS; + y_rot *= Math.toRadians(VisionConstants.V_FOV); + y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; + y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + + double x_rot = point2d.x / VisionConstants.LIME_HIXELS; + x_rot *= Math.toRadians(VisionConstants.H_FOV); + x_rot -= Math.toRadians(VisionConstants.H_FOV) / 2; + + double z = VisionConstants.TARGET_HEIGHT / Math.tan(y_rot); + double x = z * Math.tan(x_rot); + double y = VisionConstants.TARGET_HEIGHT; + + points3d.add(new Point3(x, y, z)); + } + + return points3d; + } + + /** Flattens 3d points from above + * + * @param points3d 3d points along the target rim + * @return An array of flattened 3d points + */ + public static final ArrayList topView(ArrayList points3d) { + ArrayList points = new ArrayList<>(); + + for(Point3 point3d : points3d) { + points.add(new Point(point3d.x, point3d.z)); + } + + return points; + } + + /** Finds the average point from an array of points + * + * @param points The points the average will be taken from + * @return The average point + */ + public static final Point averagePoint(ArrayList points) { + Point average = new Point(0, 0); + for(Point point : points) { + average.x += point.x; + average.y += point.y; + } + + average.x /= points.size(); + average.y /= points.size(); + + return average; + } + + /** Iterates the current guess for the vision center (relative to the limelight) + * based on points on the rim of the target + *

+ * The guess is iterated by finding the current average vector error between the guess + * and the circlePoints, assuming that the guess should be a constant radius from each point + * + * @param guess The current estimate for the vision center + * @param circlePoints Vision points along the rim of the target + * @return The guess after iteration + */ + public static final Point iterateGuess(Point guess, ArrayList circlePoints) { + Point totalDiff = new Point(0, 0); + + for(Point circlePoint : circlePoints) { + double angle = Math.atan((guess.y - circlePoint.y) / (guess.x - circlePoint.x)); + angle = correctQuadrent(angle, guess, circlePoint); + + Point estimate = new Point(); + estimate.x = VisionConstants.TARGET_RADIUS * Math.cos(angle) + guess.x; + estimate.y = VisionConstants.TARGET_RADIUS * Math.sin(angle) + guess.y; + + Point diff = new Point(estimate.x - circlePoint.x, estimate.y - circlePoint.y); + totalDiff.x += diff.x; + totalDiff.y += diff.y; + } + + totalDiff.x /= circlePoints.size(); + totalDiff.y /= circlePoints.size(); + + return new Point(guess.x - totalDiff.x, guess.y - totalDiff.y); + } + + /** Corrects odometry guess for shooter angle + * + * @param guess The current guess for the vision center + * @param shooterRotation The rotation to correct for + * @return The corrected odometry point + */ + public static final Point correctGuessForCenter(Point guess, double shooterRotation) { + Point corrected = new Point(guess.x, guess.y); + corrected.y += VisionConstants.LIMELIGHT_RADIUS; + + double dist = Math.hypot(guess.x, guess.y); + double angle = Math.tan(corrected.y / corrected.x); + angle += Math.toRadians(shooterRotation); + + corrected.x = dist * Math.cos(angle); + corrected.y = dist * Math.sin(angle); + + corrected.y += VisionConstants.SHOOTER_CORRECTION; + + return corrected; + } + + /** Corrects odometry guess for gyro angle + * + * @param guess The current guess for the vision center + * @param gyroRotation The rotation to correct for + * @return The corrected odometry point + */ + public static final Point correctGuessForGyro(Point guess, double gyroRotation) { + Point corrected = new Point(guess.x, guess.y); + + double dist = Math.hypot(guess.x, guess.y); + double angle = Math.tan(corrected.y / corrected.x); + angle += Math.toRadians(gyroRotation); + + corrected.x = dist * Math.cos(angle); + corrected.y = dist * Math.sin(angle); + + return corrected; + } + + /** Corrects the angle from the current center estimate to a point on the target rim + * for multiple quadrents + * + * @param angle The angle to be corrected + * @param guess The current guess for the vision center + * @param circlePoint A point along the target rim + * @return The angle corrected for the quadrent + */ + public static final double correctQuadrent(double angle, Point guess, Point circlePoint) { + if(circlePoint.x - guess.x < 0) { + return angle - Math.PI; + } + + return angle; + } +} diff --git a/src/main/java/frc4388/utility/VisionObscuredException.java b/src/main/java/frc4388/utility/VisionObscuredException.java new file mode 100644 index 0000000..cc33066 --- /dev/null +++ b/src/main/java/frc4388/utility/VisionObscuredException.java @@ -0,0 +1,38 @@ +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); + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..9a31b1e --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2022.1.5", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2022.1.5", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxx86-64" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2022.1.5" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2022.1.5" + } + ] +} \ No newline at end of file