diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 77e7b3e..4cd9eb7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,8 +52,6 @@ public class RobotContainer { private SendableChooser autoChooser; - private Vision vision; - private IntSupplier closestTag; public RobotContainer() { @@ -63,8 +61,6 @@ public class RobotContainer { drivetrain = new Drivetrain(); - vision = new Vision(); - elevator = new Elevator(); //elevator = new ElevatorSysID(); @@ -321,12 +317,12 @@ public class RobotContainer { //sensorTab.add("odometry", drivetrain::getPose); - apriltagTab.addDouble("Orange ID", vision::getOrangeClosestTag); - apriltagTab.addDouble("Orange tx", vision::getOrangeTX); - apriltagTab.addDouble("Orange ty", vision::getOrangeTY); - apriltagTab.addDouble("Orange dist", vision::getOrangeDist); - apriltagTab.addDouble("global x", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX()); - apriltagTab.addDouble("global y", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY()); + apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag()); + apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX()); + apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY()); + apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist()); + apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX()); + apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY()); // sensorTab.addDouble(" ID", vision::getOrangeClosestTag); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 11d1615..0250782 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -9,6 +9,8 @@ import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.Orchestra; import com.pathplanner.lib.auto.AutoBuilder; import com.studica.frc.AHRS; @@ -25,6 +27,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -33,6 +36,7 @@ import frc.robot.constants.AutoConstants; import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.OIConstants; import frc.robot.constants.VisionConstants; +import edu.wpi.first.math.geometry.Pose3d; public class Drivetrain extends SubsystemBase { // Create MAXSwerveModules @@ -56,6 +60,8 @@ public class Drivetrain extends SubsystemBase { private PIDController pidTranslationX; private PIDController pidTranslationY; + public Vision vision; + /** Creates a new DriveSubsystem. */ public Drivetrain() { m_frontLeft = new MAXSwerveModule( @@ -133,6 +139,8 @@ public class Drivetrain extends SubsystemBase { m_orchestra.play(); musicTimer.reset(); musicTimer.start(); + + vision = new Vision(); } @Override @@ -149,22 +157,30 @@ public class Drivetrain extends SubsystemBase { gyroBuffer.addSample(getGyroValue(), Timer.getTimestamp()); + // if the detected tags match your alliances reef tags use their pose estimates + if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().equals(Alliance.Red) && vision.getOrangeTagDetected()){ + Pose2d orangePose = vision.getOrangeGlobalPose(gyroBuffer); + Logger.recordOutput("orange pose", new Pose3d(orangePose)); + m_estimator.addVisionMeasurement(orangePose, vision.getOrangeTimeStamp()); - // if the detected tags match your alliances reef tags use their pose estimates - /* - if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){ - m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp()); - - }else if(vision.getOrangeClosestTag() >= 17 || vision.getOrangeClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){ - m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp()); + }else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().equals(Alliance.Blue) && vision.getOrangeTagDetected()){ + Pose2d orangePose = vision.getOrangeGlobalPose(gyroBuffer); + Logger.recordOutput("orange pose", new Pose3d(orangePose)); + m_estimator.addVisionMeasurement(orangePose, vision.getOrangeTimeStamp()); } - if(vision.getBlackClosestTag() >= 6 || vision.getBlackClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){ - m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp()); - }else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){ - m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp()); + if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().equals(Alliance.Red) && vision.getOrangeTagDetected()){ + Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer); + Logger.recordOutput("black pose", new Pose3d(blackPose)); + m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp()); + + }else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().equals(Alliance.Blue) && vision.getOrangeTagDetected()){ + Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer); + Logger.recordOutput("black pose", new Pose3d(blackPose)); + m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp()); } - */ + + Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition())); if(musicTimer.get()>20){ if (m_orchestra.isPlaying()) { @@ -369,10 +385,6 @@ public class Drivetrain extends SubsystemBase { return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); } - public void addVisionMeasurement(Pose2d pose, double timestamp){ - m_estimator.addVisionMeasurement(pose, timestamp); - } - public double driveDistance(){ return m_frontLeft.getTotalDist(); } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 7d62b32..7da2f87 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -1,14 +1,10 @@ package frc.robot.subsystems; -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.estimator.PoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTable; @@ -18,9 +14,9 @@ import frc.robot.constants.VisionConstants; public class Vision{ - private DoubleSubscriber blackRobotRelativeX; - private DoubleSubscriber blackRobotRelativeY; - private DoubleSubscriber blackRobotRelativeZ; + private DoubleSubscriber black_tx; + private DoubleSubscriber black_ty; + private DoubleSubscriber black_dist; private DoubleSubscriber blackClosestTag; private BooleanSubscriber blackTagDetected; @@ -36,8 +32,8 @@ public class Vision{ private DoubleSubscriber orangeFramerate; - private double[] orangeCamPose = {0,0,0,0,0}; - private double[] blackCamPose = {0,0,0,0,0}; + private double[] orangeCamPose = {0.0, -5.0, -10, 14.0-7.673, 1.05-14.0, 7.308+2.75}; + private double[] blackCamPose = {0.0, -5.0, 10, 14.0-7.673, 1.05-14.0, 7.308+2.75}; public Vision(){ NetworkTableInstance inst = NetworkTableInstance.getDefault(); @@ -45,9 +41,9 @@ public class Vision{ NetworkTable blackVisionTable = inst.getTable("black_Fiducial"); NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial"); - blackRobotRelativeX = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0); - blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0); - blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0); + black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0); + black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0); + black_dist = blackVisionTable.getDoubleTopic("totalDist").subscribe(0.0); blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0); blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false); @@ -79,13 +75,13 @@ public class Vision{ return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get())); } - public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer gyroBuffer){ + public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer gyroBuffer, double[] camPose){ System.out.println(gyroBuffer.getSample(timestamp)); - double distance2d = totalDist * Math.cos(-orangeCamPose[1] - ty); + double distance2d = totalDist * Math.cos(-camPose[1] - ty); - Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0] - tx)); + Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[0] - tx)); Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0], VisionConstants.globalTagCoords[tagID][1], @@ -95,8 +91,8 @@ public class Vision{ .transformBy(new Transform2d(distance2d, 0.0, new Rotation2d())) .getTranslation(); Pose2d robotPose = new Pose2d(fieldToCameraTranslation, - new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0]))) - .transformBy(new Transform2d(new Pose2d(new Translation2d(), new Rotation2d(orangeCamPose[0])), Pose2d.kZero)); + new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[0]))) + .transformBy(new Transform2d(new Pose2d(new Translation2d(), new Rotation2d(camPose[0])), Pose2d.kZero)); robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d( gyroBuffer.getSample(timestamp).get())); @@ -105,19 +101,19 @@ public class Vision{ public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer gyroBuffer){ return CameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(), - getBlackRelativeX(), getBlackRelativeY(), Timer.getTimestamp(), gyroBuffer); + getBlackTX(), getBlackTY(), Timer.getTimestamp(), gyroBuffer, blackCamPose); } - public double getBlackRelativeX(){ - return blackRobotRelativeX.get(); + public double getBlackTX(){ + return black_tx.get(); } - public double getBlackRelativeY(){ - return blackRobotRelativeY.get(); + public double getBlackTY(){ + return black_ty.get(); } - public double getBlackRelativeZ(){ - return blackRobotRelativeZ.get(); + public double getBlackDist(){ + return black_dist.get(); } public int getBlackClosestTag(){ @@ -125,7 +121,7 @@ public class Vision{ } public double getBlackTimeStamp(){ - return blackRobotRelativeX.getLastChange(); + return black_tx.getLastChange(); } public boolean getBlackTagDetected(){ @@ -139,7 +135,7 @@ public class Vision{ public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer gyroBuffer){ if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){ return CameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(), - orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer + orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer, orangeCamPose ); }else{ return new Pose2d();