diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 17750137..30b1d43d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,12 +46,12 @@ public static enum Mode { } public static final class FeatureFlags { - public static final boolean runVision = true; + public static final boolean runVision = false; - public static final boolean runIntake = true; - public static final boolean runScoring = true; - public static final boolean runEndgame = true; - public static final boolean runDrive = true; + public static final boolean runIntake = false; + public static final boolean runScoring = false; + public static final boolean runEndgame = false; + public static final boolean runDrive = false; public static final boolean enableLEDS = false; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cd1b18ec..f79aced9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,7 +41,7 @@ public void robotInit() { if (Constants.currentMode == Constants.Mode.REAL) { // TODO: Log data to a USB drive on the RIO - Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") + // Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables pdh = new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging } else if (Constants.currentMode == Constants.Mode.SIM) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 214c3abc..4469345d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc.robot.subsystems.localization.CameraContainerReal; import frc.robot.subsystems.localization.CameraContainerReplay; import frc.robot.subsystems.localization.CameraContainerSim; +import frc.robot.subsystems.localization.NoteDetection; import frc.robot.subsystems.localization.VisionLocalizer; import frc.robot.subsystems.scoring.AimerIO; import frc.robot.subsystems.scoring.AimerIORoboRio; @@ -84,6 +85,8 @@ public class RobotContainer { LED leds; + NoteDetection noteDetection; + public RobotContainer() { configureSubsystems(); configureModes(); @@ -219,6 +222,8 @@ public void configureSubsystems() { } if (FeatureFlags.enableLEDS) leds = new LED(scoringSubsystem); + + noteDetection = new NoteDetection(() -> new Pose2d(0, 0, new Rotation2d())); } // spotless:off @@ -240,6 +245,10 @@ private void configureBindings() { () -> drivetrain.seedFieldRelative(getPoseAgainstSpeaker())) ); + controller.a() + .onTrue(new InstantCommand( + () -> drivetrain.setAlignTarget(AlignTarget.NOTE))); + controller.b() .onTrue(new InstantCommand( () -> drivetrain.setAlignTarget(AlignTarget.SPEAKER))); @@ -679,11 +688,11 @@ public void robotPeriodic() { public void disabledPeriodic() { /* set to coast mode when circuit open */ if (brakeSwitch != null && brakeSwitch.get()) { - scoringSubsystem.setBrakeMode(false); - endgameSubsystem.setBrakeMode(false); + if (FeatureFlags.runScoring) scoringSubsystem.setBrakeMode(false); + if (FeatureFlags.runEndgame) endgameSubsystem.setBrakeMode(false); } else { - scoringSubsystem.setBrakeMode(true); - endgameSubsystem.setBrakeMode(true); + if (FeatureFlags.runScoring) scoringSubsystem.setBrakeMode(true); + if (FeatureFlags.runEndgame) endgameSubsystem.setBrakeMode(true); } } diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java index 1f0f3c90..ada9c32b 100644 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java @@ -20,6 +20,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -29,8 +31,11 @@ import frc.robot.Constants.TunerConstants; import frc.robot.utils.GeomUtil; import frc.robot.utils.InterpolateDouble; +import java.util.List; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.TargetCorner; /** * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used @@ -42,8 +47,14 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst private double alignError = 0.0; + private Timer elapsedDriveToNote = new Timer(); + private double estimatedTimeToNote = 0; + private boolean drivingToNote = false; + private double allotedErrorTime = 2; + public enum AlignTarget { NONE, + NOTE, AMP, SPEAKER, SOURCE @@ -57,6 +68,8 @@ public enum AlignState { private AlignTarget alignTarget = AlignTarget.NONE; private AlignState alignState = AlignState.MANUAL; + private PhotonCamera colorCamera = new PhotonCamera("photonvision-orange"); + private static InterpolateDouble noteTimeToGoal = new InterpolateDouble(ScoringConstants.timeToGoalMap(), 0.0, 2.0); @@ -68,6 +81,8 @@ public enum AlignState { private Supplier getRobotVelocity = () -> new Translation2d(); + private Supplier hasNote = () -> false; + private PIDController thetaController = new PIDController( DriveConstants.alignmentkPMax, @@ -76,7 +91,7 @@ public enum AlignState { private SwerveRequest.FieldCentric driveFieldCentric = new SwerveRequest.FieldCentric(); private SwerveRequest.RobotCentric driveRobotCentric = new SwerveRequest.RobotCentric(); - private SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + // private SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private static final double kSimLoopPeriod = 0.02; // Original: 5 ms private Notifier simNotifier = null; @@ -101,6 +116,8 @@ public CommandSwerveDrivetrain( * In short, never trust the Command Scheduler. */ CommandScheduler.getInstance().registerSubsystem(this); + elapsedDriveToNote.reset(); + elapsedDriveToNote.stop(); } public CommandSwerveDrivetrain( @@ -138,6 +155,10 @@ public void setSourceSupplier(Supplier getFieldToSource) { this.getFieldToSource = getFieldToSource; } + public void setNoteAcquiredSupplier(Supplier hasNote) { + this.hasNote = hasNote; + } + public void setAlignTarget(AlignTarget alignTarget) { this.alignTarget = alignTarget; } @@ -233,8 +254,48 @@ public void setGoalChassisSpeeds(ChassisSpeeds chassisSpeeds) { private void controlDrivetrain() { Pose2d pose = getFieldToRobot.get(); Rotation2d desiredHeading = pose.getRotation(); + + if (alignState != AlignState.ALIGNING || alignTarget != AlignTarget.NOTE) { + drivingToNote = false; + elapsedDriveToNote.stop(); + elapsedDriveToNote.reset(); + } + if (alignState == AlignState.ALIGNING) { switch (alignTarget) { + /*case NOTE: + var cameraResult = colorCamera.getLatestResult(); + if (cameraResult.hasTargets()) { + Pose2d notePose = getNotePoseFromTarget(cameraResult.getBestTarget()); + + if (!drivingToNote) { + + double distance = + Math.sqrt( + Math.pow(notePose.getX(), 2) + + Math.pow(notePose.getY(), 2)); + + drivingToNote = true; + estimatedTimeToNote = + distance / DriveConstants.MaxSpeedMetPerSec + allotedErrorTime; + elapsedDriveToNote.reset(); + elapsedDriveToNote.start(); + + setGoalChassisSpeeds( + new ChassisSpeeds(DriveConstants.MaxSpeedMetPerSec, 0, 0), + false); + desiredHeading = notePose.getRotation(); + } else if (elapsedDriveToNote.get() > estimatedTimeToNote + || hasNote.get()) { + setGoalChassisSpeeds(new ChassisSpeeds()); + } else { + setGoalChassisSpeeds( + new ChassisSpeeds(DriveConstants.MaxSpeedMetPerSec, 0, 0), + false); + desiredHeading = notePose.getRotation(); + } + } + break;*/ case AMP: desiredHeading = getFieldToAmp.get(); break; @@ -347,5 +408,16 @@ public boolean isAligned() { @Override public void periodic() { controlDrivetrain(); + + List corners = + colorCamera.getLatestResult().getBestTarget().getDetectedCorners(); + double[][] cornersCoordinates = new double[4][2]; + /*for (int i = 0; i < 4; i++) { + cornersCoordinates[i] = getCoordinateOfTargetCorner(corners.get(i)); + }*/ + SmartDashboard.putNumberArray("coordinates 0", cornersCoordinates[0]); + SmartDashboard.putNumberArray("coordinates 1", cornersCoordinates[1]); + SmartDashboard.putNumberArray("coordinates 2", cornersCoordinates[2]); + SmartDashboard.putNumberArray("coordinates 3", cornersCoordinates[3]); } } diff --git a/src/main/java/frc/robot/subsystems/localization/NoteDetection.java b/src/main/java/frc/robot/subsystems/localization/NoteDetection.java new file mode 100644 index 00000000..63a00745 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/localization/NoteDetection.java @@ -0,0 +1,84 @@ +package frc.robot.subsystems.localization; + +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 java.util.List; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +public class NoteDetection extends SubsystemBase { + + private PhotonCamera colorCamera = new PhotonCamera("note-detector"); + private Supplier robotPose; + + public NoteDetection(Supplier robotPose) { + this.robotPose = robotPose; + SmartDashboard.putBoolean("running", true); + } + + private Pose2d getNotePoseFromTarget(PhotonTrackedTarget target) { + double transformWidth = 1 * 78; // = distance at setpoint / width at setpoint + double noteWidth = 0.1; + // w of real note = 35 cm + // .405 * 74 * 35 = d * wd + + List corners = target.getMinAreaRectCorners(); + + double[][] cornersCoordinates = new double[4][2]; + SmartDashboard.putNumber("corners", corners.size()); + SmartDashboard.putNumber("area", target.getArea()); + for (int i = 0; i < corners.size(); i++) { + cornersCoordinates[i][0] = corners.get(i).x; + cornersCoordinates[i][1] = corners.get(i).y; + SmartDashboard.putNumber("coordinate" + (i) + "x", cornersCoordinates[i][0]); + SmartDashboard.putNumber("coordinate" + (i) + "y", cornersCoordinates[i][1]); + } + + double width = cornersCoordinates[1][0] - cornersCoordinates[0][0]; + double distanceForward = transformWidth / width; + // alternatively: instead of using magnification maybe try using just y instead? + SmartDashboard.putNumber("distanceForward", distanceForward); + // THEORETICALLY WORKING + + double widthShift = 0.180 * 199 / 2; + double distanceSide = + widthShift + * noteWidth + / ((cornersCoordinates[1][0] + cornersCoordinates[0][0]) / 2 - 160) + / distanceForward; + SmartDashboard.putNumber("distanceSize", distanceSide); + + double distance = + Math.sqrt(distanceForward * distanceForward + distanceSide * distanceSide); + double angle = Math.tanh(distanceSide / distanceForward); + + Rotation2d fromForward = robotPose.get().getRotation().plus(new Rotation2d(angle)); + + SmartDashboard.putNumber("width", width); + SmartDashboard.putNumber( + "centerx", (cornersCoordinates[1][0] + cornersCoordinates[0][0]) / 2); + SmartDashboard.putNumber( + "centery", (cornersCoordinates[1][1] + cornersCoordinates[0][1]) / 2); + + return new Pose2d( + robotPose.get().getX() + Math.cos(fromForward.getRadians()) * distance, + robotPose.get().getY() + Math.sin(fromForward.getRadians()) * distance, + fromForward); + } + + @Override + public void periodic() { + SmartDashboard.putBoolean("hello", true); + var cameraResult = colorCamera.getLatestResult(); + if (cameraResult.hasTargets()) { + Pose2d notePose = getNotePoseFromTarget(cameraResult.getBestTarget()); + Logger.recordOutput("Note Position", notePose); + SmartDashboard.putBoolean("target found", true); + } + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 26a28d5d..0e80a16c 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.7", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.7", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.7", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.7" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.7" + "version": "v2024.3.1" } ] } \ No newline at end of file