diff --git a/src/main/deploy/pathplanner/autos/Left Trench shoot and Human Player.auto b/src/main/deploy/pathplanner/autos/Left Trench shoot and Human Player.auto index 8707c8b..9922e6e 100644 --- a/src/main/deploy/pathplanner/autos/Left Trench shoot and Human Player.auto +++ b/src/main/deploy/pathplanner/autos/Left Trench shoot and Human Player.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "LT to Human Player" + "pathName": "Red LT to Human Player" } }, { @@ -37,7 +37,7 @@ { "type": "path", "data": { - "pathName": "Human Player to Tower" + "pathName": "Red Human Player to Tower" } }, { diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 1c0319f..4ec6b50 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -7,38 +7,39 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableInstance; public final class Constants { - public static NetworkTableInstance NT_INSTANCE; public final class VisionConstants { - - public static String CAMERA_FR_NAME; - public static double CAMERA_FR_YAW; - public static double CAMERA_FR_PITCH; - public static double CAMERA_FR_ROLL; - public static double CAMERA_FR_FORWARD; - public static double CAMERA_FR_LEFT; - public static double CAMERA_FR_UP; - - public static String CAMERA_FL_NAME; - public static double CAMERA_FL_YAW; - public static double CAMERA_FL_PITCH; - public static double CAMERA_FL_ROLL; - public static double CAMERA_FL_FORWARD; - public static double CAMERA_FL_LEFT; - public static double CAMERA_FL_UP; - - public static String CAMERA_BR_NAME; - public static double CAMERA_BR_YAW; - public static double CAMERA_BR_PITCH; - public static double CAMERA_BR_ROLL; - public static double CAMERA_BR_FORWARD; - public static double CAMERA_BR_LEFT; - public static double CAMERA_BR_UP; + public static final NetworkTableInstance NT_INSTANCE = NetworkTableInstance.getDefault(); + + public static final String CAMERA_FR_NAME = "CAMERA_FR"; + public static final double CAMERA_FR_YAW = 0; + public static final double CAMERA_FR_PITCH = 28.812; + public static final double CAMERA_FR_ROLL = 0; + public static final double CAMERA_FR_FORWARD = Units.inchesToMeters(15.03); + public static final double CAMERA_FR_LEFT = Units.inchesToMeters(-1.475); + public static final double CAMERA_FR_UP = Units.inchesToMeters(20); + + public static final String CAMERA_FL_NAME = "CAMERA_FL"; + public static final double CAMERA_FL_YAW = 45; + public static final double CAMERA_FL_PITCH = 14.864; + public static final double CAMERA_FL_ROLL = 0; + public static final double CAMERA_FL_FORWARD = Units.inchesToMeters(9.893); + public static final double CAMERA_FL_LEFT = Units.inchesToMeters(-7.310); + public static final double CAMERA_FL_UP = Units.inchesToMeters(20); + + public static final String CAMERA_BR_NAME = "CAMERA_BR"; + public static final double CAMERA_BR_YAW = 0; + public static final double CAMERA_BR_PITCH = 0; + public static final double CAMERA_BR_ROLL = 0; + public static final double CAMERA_BR_FORWARD = 0; + public static final double CAMERA_BR_LEFT = 0; + public static final double CAMERA_BR_UP = 0; @@ -46,23 +47,14 @@ public final class VisionConstants { // The standard deviations of our vision estimated poses, which affect correction rate // (Fake values. Experiment and determine estimation noise on an actual robot.) - public static double MAX_VALID_DIST; - public static Matrix kSINGLE_TAG_STD_DEVS; - public static Matrix kMULTI_TAG_STD_DEVS; - public static Matrix ODOM_STD_DEV; + public static final double MAX_VALID_DIST = 5.0; //Meters + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + public static final Matrix ODOM_STD_DEV = VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(0.01)); } - public final class SwerveConstants { - public static Translation2d k_frontLeftLocation; - public static Translation2d k_frontRightLocation; - public static Translation2d k_backLeftLocation; - public static Translation2d k_backRightLocation; - } - - - public final class LauncherConstants { public static double SHOOTER_SIZE_IN = 7.0; @@ -77,4 +69,4 @@ public final class LauncherConstants { public static double TOWER_HOOD_ANGLE = 1.25; public static double TOWER_RPM = -2250.0; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Swerve.java b/src/main/java/frc/robot/subsystems/drivetrain/Swerve.java index e3ee849..65863b2 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Swerve.java @@ -29,7 +29,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.constants.TunerConstants; import frc.robot.constants.TunerConstants.TunerSwerveDrivetrain; -//import frc.robot.subsystems.localizer.Localizer; +import frc.robot.subsystems.localizer.Localizer; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; @@ -60,7 +60,7 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - //private Localizer localizer; + private Localizer localizer; /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; @@ -157,7 +157,7 @@ public Swerve( if (Utils.isSimulation()) { startSimThread(); } - //localizer = new Localizer(this); + localizer = new Localizer(this); // Configure AutoBuilder last configureAutoBuilder(); } @@ -217,7 +217,7 @@ public Swerve( if (Utils.isSimulation()) { startSimThread(); } - //localizer = new Localizer(this); + localizer = new Localizer(this); configureAutoBuilder(); } @@ -251,7 +251,7 @@ public Swerve( if (Utils.isSimulation()) { startSimThread(); } - //localizer = new Localizer(this); + localizer = new Localizer(this); configureAutoBuilder(); } @@ -309,7 +309,7 @@ public void periodic() { }); } - //localizer.periodic(); + localizer.periodic(); } private void startSimThread() { diff --git a/src/main/java/frc/robot/subsystems/localizer/Localizer.java b/src/main/java/frc/robot/subsystems/localizer/Localizer.java index b136cba..1ea6d07 100644 --- a/src/main/java/frc/robot/subsystems/localizer/Localizer.java +++ b/src/main/java/frc/robot/subsystems/localizer/Localizer.java @@ -1,92 +1,91 @@ -// package frc.robot.subsystems.localizer; - -// import java.util.List; -// import java.util.Optional; - - -// import org.photonvision.EstimatedRobotPose; -// import org.photonvision.PhotonCamera; -// import org.photonvision.PhotonPoseEstimator; -// import org.photonvision.PhotonUtils; -// import org.photonvision.targeting.PhotonPipelineResult; -// import org.photonvision.targeting.PhotonTrackedTarget; - -// import edu.wpi.first.apriltag.AprilTagFieldLayout; -// import edu.wpi.first.apriltag.AprilTagFields; -// import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -// import edu.wpi.first.math.geometry.*; -// import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -// import edu.wpi.first.math.kinematics.SwerveModulePosition; -// import edu.wpi.first.math.numbers.N1; -// import edu.wpi.first.math.numbers.N3; -// import edu.wpi.first.networktables.NetworkTable; -// import edu.wpi.first.networktables.NetworkTableInstance; -// import edu.wpi.first.networktables.StructPublisher; -// import edu.wpi.first.wpilibj.smartdashboard.Field2d; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; -// import edu.wpi.first.math.Matrix; -// import frc.robot.Robot; -// import frc.robot.constants.Constants; -// import frc.robot.subsystems.drivetrain.Swerve; -// import frc.robot.util.vision.Vision.BW; -// import frc.robot.util.vision.Vision.BW.BWCamera; -// import frc.robot.util.vision.Vision; - -// public class Localizer extends SubsystemBase { - -// private final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - -// private final Vision vision; -// private final SwerveDrivePoseEstimator poseEstimator; +package frc.robot.subsystems.localizer; + +import java.util.List; +import java.util.Optional; + + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonUtils; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.Matrix; +import frc.robot.Robot; +import frc.robot.constants.Constants; +import frc.robot.subsystems.drivetrain.Swerve; +import frc.robot.util.vision.Vision.BW; +import frc.robot.util.vision.Vision.BW.BWCamera; +import frc.robot.util.vision.Vision; + +public class Localizer extends SubsystemBase { + + private final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + private final Vision vision; + private final SwerveDrivePoseEstimator poseEstimator; -// private final Swerve swerve; + private final Swerve swerve; -// public Field2d fieldOdom; + public Field2d fieldOdom; -// private Pose2d currentPose; + private Pose2d currentPose; + public Localizer(Swerve swerve) { -// public Localizer(Swerve swerve) { + currentPose = new Pose2d(); + fieldOdom = new Field2d(); -// currentPose = new Pose2d(); -// fieldOdom = new Field2d(); + this.swerve = swerve; -// this.swerve = swerve; - -// poseEstimator = new SwerveDrivePoseEstimator( -// this.swerve.getKinematics(), -// this.swerve.getState().RawHeading, -// this.swerve.getState().ModulePositions, -// this.swerve.getState().Pose, -// Constants.VisionConstants.ODOM_STD_DEV, -// Constants.VisionConstants.kMULTI_TAG_STD_DEVS -// ); + poseEstimator = new SwerveDrivePoseEstimator( + this.swerve.getKinematics(), + this.swerve.getState().RawHeading, + this.swerve.getState().ModulePositions, + this.swerve.getState().Pose, + Constants.VisionConstants.ODOM_STD_DEV, + Constants.VisionConstants.kMultiTagStdDevs + ); -// vision = new Vision((pose, timestamp, stdDevs) -> { -// poseEstimator.addVisionMeasurement(pose, timestamp, stdDevs); -// }); - -// //TEMP, unsure if correct. -// SmartDashboard.putData("Field2d Pose", fieldOdom); + vision = new Vision((pose, timestamp, stdDevs) -> { + poseEstimator.addVisionMeasurement(pose, timestamp, stdDevs); + }); + //SmartDashboard.putData("Field2d Pose", fieldOdom); -// } + } -// public void resetPose(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d newPose) { -// poseEstimator.resetPosition(gyroAngle, modulePositions, newPose); -// } + public void resetPose(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d newPose) { + poseEstimator.resetPosition(gyroAngle, modulePositions, newPose); + } -// public void updateOdometry(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { -// poseEstimator.update(gyroAngle, modulePositions); -// } + public void updateOdometry(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { + poseEstimator.update(gyroAngle, modulePositions); + } -// public void periodic() { -// currentPose = poseEstimator.getEstimatedPosition(); -// vision.getEstimatedGlobalPoses(currentPose); + public void periodic() { + poseEstimator.update(this.swerve.getState().RawHeading, this.swerve.getState().ModulePositions); + vision.getEstimatedGlobalPoses(poseEstimator.getEstimatedPosition()); + currentPose = poseEstimator.getEstimatedPosition(); + fieldOdom.setRobotPose(currentPose); + SmartDashboard.putData("Field2d Pose", fieldOdom); -// fieldOdom.setRobotPose(currentPose); -// } + } -// } + } diff --git a/src/main/java/frc/robot/util/vision/Vision.java b/src/main/java/frc/robot/util/vision/Vision.java index cd018dc..3152ff9 100644 --- a/src/main/java/frc/robot/util/vision/Vision.java +++ b/src/main/java/frc/robot/util/vision/Vision.java @@ -49,14 +49,14 @@ public static interface EstimateConsumer { private final Map estimators = new EnumMap<>(BWCamera.class); - private final NetworkTable visionTable; + //private final NetworkTable visionTable; private final Field2d debugField = new Field2d(); //TODO: PUBLISH VALUES TO NETWORK TABLES public Vision(EstimateConsumer estC) { - visionTable = NetworkTableInstance.getDefault().getTable("Vision"); + //visionTable = NetworkTableInstance.getDefault().getTable("Vision"); for(BWCamera cam : BW.BWCamera.values()) { PhotonPoseEstimator estimator = new PhotonPoseEstimator(kTagLayout, cam.robotToCameraOffset); @@ -70,11 +70,11 @@ public Vision(EstimateConsumer estC) { private void updateEstimationStdDevs(Optional estimatedPose, List targets, PhotonPoseEstimator photonEstimator) { if (estimatedPose.isEmpty()) { // No pose input. Default to single-tag std devs - curStdDevs = Constants.VisionConstants.kSINGLE_TAG_STD_DEVS; + curStdDevs = Constants.VisionConstants.kSingleTagStdDevs; } else { // Pose present. Start running Heuristic - var estStdDevs = Constants.VisionConstants.kSINGLE_TAG_STD_DEVS; + var estStdDevs = Constants.VisionConstants.kSingleTagStdDevs; int numTags = 0; double avgDist = 0; @@ -93,12 +93,12 @@ private void updateEstimationStdDevs(Optional estimatedPose, if (numTags == 0) { // No tags visible. Default to single-tag std devs - curStdDevs = Constants.VisionConstants.kSINGLE_TAG_STD_DEVS; + curStdDevs = Constants.VisionConstants.kSingleTagStdDevs; } else { // One or more tags visible, run the full heuristic. avgDist /= numTags; // Decrease std devs if multiple targets are visible - if (numTags > 1) estStdDevs = Constants.VisionConstants.kMULTI_TAG_STD_DEVS; + if (numTags > 1) estStdDevs = Constants.VisionConstants.kMultiTagStdDevs; // Increase std devs based on (average) distance if (numTags == 1 && avgDist > 4) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); @@ -191,7 +191,7 @@ public void processTargets(PhotonPipelineResult result){ public static final class BW { public enum BWCamera { CAMERA_FR ( - new PhotonCamera(Constants.NT_INSTANCE, Constants.VisionConstants.CAMERA_FR_NAME), + new PhotonCamera(Constants.VisionConstants.NT_INSTANCE, Constants.VisionConstants.CAMERA_FR_NAME), new Transform3d( Constants.VisionConstants.CAMERA_FR_FORWARD, Constants.VisionConstants.CAMERA_FR_LEFT, @@ -205,7 +205,7 @@ public enum BWCamera { ), CAMERA_FL ( - new PhotonCamera(Constants.NT_INSTANCE, Constants.VisionConstants.CAMERA_FL_NAME), + new PhotonCamera(Constants.VisionConstants.NT_INSTANCE, Constants.VisionConstants.CAMERA_FL_NAME), new Transform3d( Constants.VisionConstants.CAMERA_FL_FORWARD, Constants.VisionConstants.CAMERA_FL_LEFT, @@ -219,7 +219,7 @@ public enum BWCamera { ), CAMERA_BR ( - new PhotonCamera(Constants.NT_INSTANCE, Constants.VisionConstants.CAMERA_BR_NAME), + new PhotonCamera(Constants.VisionConstants.NT_INSTANCE, Constants.VisionConstants.CAMERA_BR_NAME), new Transform3d( Constants.VisionConstants.CAMERA_BR_FORWARD, Constants.VisionConstants.CAMERA_BR_LEFT,