diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 80ad347..9fa45e3 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -150,4 +150,23 @@ public final class Constants { public static final double RIGHT_AXIS_DEADBAND = 0.6; public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing } + + public static final class VisionConstants { + public static final String NAME = "photonCamera"; + + public static final int LIME_HIXELS = 640; + public static final int LIME_VIXELS = 480; + + public static final double H_FOV = 59.6; + public static final double V_FOV = 45.7; + + public static final double LIME_HEIGHT = 6.0; + public static final double LIME_ANGLE = 55.0; + + // public static final double HIGH_TARGET_HEIGHT = 46.0; + public static final double HIGH_TAPE_HEIGHT = 44.0; + + // public static final double MID_TARGET_HEIGHT = 34.0; + public static final double MID_TAPE_HEIGHT = 24.0; + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index eac33f3..77ca04b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,10 +7,6 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; - -import edu.wpi.first.wpilibj.PWM; -import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -21,9 +17,10 @@ import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Claw; +import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.commands.JoystickRecorder; -import frc4388.robot.commands.PivotCommand; +import frc4388.robot.commands.LimeAlign; import frc4388.robot.commands.PlaybackChooser; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; @@ -50,6 +47,8 @@ public class RobotContainer { public final Claw m_robotClaw = new Claw(m_robotMap.servo); + public final Limelight m_limeLight = new Limelight(); + /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -153,13 +152,15 @@ public class RobotContainer { - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new PivotCommand(m_robotArm, 135)); - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new PivotCommand(m_robotArm, 210)); + .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight)); + - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + .whileTrue(new LimeAlign(m_robotSwerveDrive, m_limeLight)); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index cace0a8..629c903 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -38,16 +38,4 @@ public class AutoBalance extends PelvicInflammatoryDisease { if (Math.abs(getError()) < 3) out2 = 0; drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); } - - @Override - public void initialize() { - super.initialize(); - // this.gyro.reset(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } } diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/LimeAlign.java index c811a3e..533b6f0 100644 --- a/src/main/java/frc4388/robot/commands/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/LimeAlign.java @@ -4,31 +4,47 @@ package frc4388.robot.commands; -import edu.wpi.first.wpilibj2.command.CommandBase; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; -public class LimeAlign extends CommandBase { - public LimeAlign(SwerveDrive drive) { - addRequirements(drive); - } +public class LimeAlign extends PelvicInflammatoryDisease { - // Called when the command is initially scheduled. - @Override - public void initialize() { + SwerveDrive drive; + Limelight lime; + + public LimeAlign(SwerveDrive drive, Limelight lime) { + super(0.7, 0.1, 0.0, 0.0, 0); + + this.drive = drive; + this.lime = lime; + addRequirements(drive, lime); } - // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public double getError() { + double err = 0.0; - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} + try { + err = lime.getFirstTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); + } catch (NullPointerException ex) {} + + return err; + } - // Returns true when the command should end. @Override - public boolean isFinished() { - return false; + public void runWithOutput(double output) { + + if (output > 0) { + output += 0.6; + } else if (output < 0) { + output -= 0.6; + } + + drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true); } } diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index 6c54024..a3c9e6c 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -4,7 +4,6 @@ package frc4388.robot.commands; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.utility.Gains; @@ -26,12 +25,13 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { /** produces the error from the setpoint */ public abstract double getError(); + /** figure it out bitch */ public abstract void runWithOutput(double output); // Called when the command is initially scheduled. @Override - public void initialize() { + public final void initialize() { output = 0; } @@ -39,7 +39,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { + public final void execute() { double error = getError(); cumError += error * .02; // 20 ms double delta = error - prevError; @@ -54,7 +54,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { // Returns true when the command should end. @Override - public boolean isFinished() { + public final boolean isFinished() { return Math.abs(getError()) < tolerance; } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java new file mode 100644 index 0000000..c5f3c50 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -0,0 +1,134 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import java.util.ArrayList; +import java.util.List; + +import org.opencv.core.Point; +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.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.VisionConstants; +import frc4388.utility.AbhiIsADumbass; + +public class Limelight extends SubsystemBase { + private PhotonCamera cam; + + private boolean lightOn; + + /** Creates a new Limelight. */ + public Limelight() { + cam = new PhotonCamera(VisionConstants.NAME); + cam.setDriverMode(false); + + setToLimePipeline(); + } + + public void setLEDs(boolean on) { + lightOn = on; + cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + public void toggleLEDs() { + lightOn = !lightOn; + cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + public void setDriverMode(boolean driverMode) { + cam.setDriverMode(driverMode); + } + + public void setToLimePipeline() { + cam.setPipelineIndex(1); + } + + public void setToAprilPipeline() { + cam.setPipelineIndex(0); + } + + public ArrayList getTargetPoints() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + ArrayList points = new ArrayList<>(2); + + for(PhotonTrackedTarget target : result.getTargets()) { + List corners = target.getDetectedCorners(); + + double sumX = 0.0; + double sumY = 0.0; + double mx = 0.0; + double my = 0.0; + + for (TargetCorner c : corners) { + sumX += c.x; + sumY += c.y; + } + + mx = sumX / 4.0; + my = sumY / 4.0; + + points.add(new Point(mx, my)); + } + + return points; + } + + public PhotonTrackedTarget getFirstTargetPoint() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + return result.getBestTarget(); + } + + private double getPointAngle(Point point) { + return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS); + } + + public double getHorizontalDistanceToTarget(boolean high) { + ArrayList targetPoints = getTargetPoints(); + if (targetPoints == null) return -1; + + // Point highPoint = targetPoints.get(0).y <= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1); + // Point midPoint = targetPoints.get(0).y >= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1); + + PhotonTrackedTarget tapePoint = getFirstTargetPoint();//high ? highPoint : midPoint; + double tapeHeight = VisionConstants.MID_TAPE_HEIGHT;//high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT; + + double theta = 35.0 + tapePoint.getPitch(); + + double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT; + + double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta)); + + return horizontalDistanceToTarget; + } + + int ctr = 0; + + @Override + public void periodic() { + // This method will be called once per scheduler run + + if (ctr % 50 == 0) { + SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false)); + } + + ctr++; + + } +} diff --git a/src/main/java/frc4388/utility/AbhiIsADumbass.java b/src/main/java/frc4388/utility/AbhiIsADumbass.java new file mode 100644 index 0000000..14a9515 --- /dev/null +++ b/src/main/java/frc4388/utility/AbhiIsADumbass.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility; + +/** Exception that occurs if the limelight can't see enough points + * @author Abhi Sachi + * @author Aarav Shah + */ +public class AbhiIsADumbass extends Exception { + /** + * Creates new AbhiIsADumbass with error text 'null' + */ + public AbhiIsADumbass() { + super("Unable to see sufficient vision points (Abhi is a dumbass)"); + } + + /** Creates new AbhiIsADumbass with error text message + * + * @param message Error text message + */ + public AbhiIsADumbass(String message) { + super(message); + } + + /** Creates new AbhiIsADumbass with error text message and detailed stack trace + * + * @param message Error text message + * @param cause Root cause of error + */ + public AbhiIsADumbass(String message, Throwable cause) { + super(message, cause); + } + + /** Creates new AbhiIsADumbass with error text 'null' and detailed stack trace + * + * @param cause Root cause of error + */ + public AbhiIsADumbass(Throwable cause) { + super("Unable to see sufficient vision points (Abhi is a dumbass)", cause); + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..dad3105 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.4.2", + "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": "v2023.4.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.4.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.4.2" + } + ] +} \ No newline at end of file