From ecfa38dcc3955a8eb61dc3fad49c0f4ac98d3d9f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Mar 2023 14:41:01 -0600 Subject: [PATCH] aprilrotalign works --- .../java/frc4388/robot/RobotContainer.java | 7 ++- .../commands/Placement/AprilRotAlign.java | 42 +++++++++++++ .../robot/commands/Placement/LimeAlign.java | 7 --- .../frc4388/robot/subsystems/Limelight.java | 59 +++++++++++++++---- 4 files changed, 94 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e4f93d2..8da0eab 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,7 +7,6 @@ package frc4388.robot; -import java.lang.StackWalker.Option; import java.util.function.Consumer; import edu.wpi.first.math.geometry.Translation2d; @@ -29,7 +28,7 @@ import frc4388.robot.commands.Arm.PivotCommand; import frc4388.robot.commands.Arm.TeleCommand; import frc4388.robot.commands.Autos.AutoBalance; import frc4388.robot.commands.Autos.PlaybackChooser; -import frc4388.robot.commands.Placement.DriveToLimeDistance; +import frc4388.robot.commands.Placement.AprilRotAlign; import frc4388.robot.commands.Placement.LimeAlign; import frc4388.robot.commands.Swerve.RotateToAngle; import frc4388.utility.controller.DeadbandedXboxController; @@ -263,8 +262,10 @@ public class RobotContainer { .onFalse(interruptCommand.asProxy()); // toggle claw + // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final + // .onTrue(toggleClaw.asProxy()); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final - .onTrue(toggleClaw.asProxy()); + .whileTrue(new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight)); // kill soft limits new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final diff --git a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java new file mode 100644 index 0000000..212d979 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java @@ -0,0 +1,42 @@ +// 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.commands.Placement; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.SwerveDrive; + +public class AprilRotAlign extends PelvicInflammatoryDisease { + + SwerveDrive drive; + Limelight lime; + + /** Creates a new AprilRotAlign. */ + public AprilRotAlign(SwerveDrive drive, Limelight lime) { + super(0.1, 0.2, 0.0, 0.0, 0.0); + + this.drive = drive; + this.lime = lime; + + addRequirements(drive, lime); + } + + @Override + public double getError() { + double err = 0.0; + + try { + err = lime.getAprilSkew(); + } catch (NullPointerException ex) {} + + return err; + } + + @Override + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true); + } +} diff --git a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java index 578c778..5b2e87f 100644 --- a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java @@ -43,13 +43,6 @@ public class LimeAlign extends PelvicInflammatoryDisease { @Override 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/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index a54488d..262b0e7 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -5,11 +5,13 @@ package frc4388.robot.subsystems; import java.util.ArrayList; +import java.util.List; 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; @@ -50,7 +52,6 @@ public class Limelight extends SubsystemBase { setLEDs(false); } - // ! might need to find midpoint instead of entire target public PhotonTrackedTarget getAprilPoint() { if (!cam.isConnected()) return null; @@ -61,6 +62,51 @@ public class Limelight extends SubsystemBase { return result.getBestTarget(); } + private List getAprilCorners() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + return result.getBestTarget().getDetectedCorners(); + } + + public double getAprilSkew() { + List corners = getAprilCorners(); + ArrayList bottomSide = getAprilBottomSide(corners); + + if (bottomSide == null) return 0; + + TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); + TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); + + return bottomLeft.y - bottomRight.y; + } + + private ArrayList getAprilBottomSide(List box) { + if (box == null) return null; + + ArrayList bottomSide = new ArrayList<>(); + + TargetCorner l1 = new TargetCorner(-1, -1); + TargetCorner l2 = new TargetCorner(-1, -1); + + for (TargetCorner c : box) { + if (c.y > l1.y) l1 = c; + } + + for (TargetCorner c : box) { + if (c.y == l1.y) continue; + if (c.y > l2.y) l2 = c; + } + + bottomSide.add(l1); + bottomSide.add(l2); + + return bottomSide; + } + public double getDistanceToApril() { PhotonTrackedTarget aprilPoint = getAprilPoint(); if (aprilPoint == null) return -1; @@ -102,17 +148,8 @@ public class Limelight extends SubsystemBase { return distanceToTape; } - int ctr = 0; - @Override public void periodic() { - // This method will be called once per scheduler run - - if (ctr % 50 == 0) { - SmartDashboard.putNumber("Horizontal Distance", getDistanceToTape()); - } - - ctr++; - + SmartDashboard.putNumber("April Skew", getAprilSkew()); } }