diff --git a/.vscode/settings.json b/.vscode/settings.json index dccbc7c..d2f7034 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/build.gradle b/build.gradle index 302477e..689ff84 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" } java { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1ebcd1a..25d3767 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -46,11 +46,13 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import frc4388.utility.CanDevice; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; +import frc4388.utility.ReefPositionHelper; import frc4388.utility.Trim; /** @@ -82,7 +84,7 @@ public final class Constants { public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED}; public static final int STARTING_GEAR = 0; - // dimensions + // Dimensions public static final double WIDTH = 18.5; // TODO: validate public static final double HEIGHT = 18.5; // TODO: validate public static final double HALF_WIDTH = WIDTH / 2.d; @@ -99,42 +101,82 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; + // Operation public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 - public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); + public static final boolean DRIFT_CORRECTION_ENABLED = true; + public static final boolean INVERT_X = true; + public static final boolean INVERT_Y = false; + public static final boolean INVERT_ROTATION = false; - private static final class ModuleSpecificConstants { //2025 + // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); + + /* private static final class ModuleSpecificConstants { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125); - private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = true; + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); + private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_HEIGHT); - private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5); - private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25); + private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; - private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_HEIGHT); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_WIDTH); + private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.5); - private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = true; + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5); + private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; - private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_HEIGHT); - private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_WIDTH); + private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.07666015625); - private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = true; + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25); + private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_HEIGHT); - private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); + } */ + + private static final class ModuleSpecificConstants { // 2024 + //Front Left + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); + private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; + private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; + private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); + + //Front Right + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); + private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; + private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; + private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); + + //Back Left + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); + private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; + private static final boolean BACK_LEFT_ENCODER_INVERTED = false; + private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); + + //Back Right + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); + private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; + private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; + private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } public static final class IDs { @@ -183,17 +225,26 @@ public final class Constants { public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs() .withKP(0.1).withKI(0).withKD(0) .withKS(0).withKV(0.124); + + public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); } public static final class AutoConstants { public static final Gains XY_GAINS = new Gains(3,0,0); public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0); + + public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole + public static final int LIDAR_DIO_CHANNEL = 0; + public static final int LIDAR_MICROS_TO_CM = 10; + public static final int SECONDS_TO_MICROS = 1000000; public static final double XY_TOLERANCE = 0.05; public static final double ROT_TOLERANCE = 1; - public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); + // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); + // public static final Pose2d targetpos = } @@ -292,23 +343,32 @@ public final class Constants { public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI)); - // public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - - // Test april tag field layout - public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( - Arrays.asList(new AprilTag[] { - new AprilTag(1, new Pose3d( - new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) - )), - }), 100, 100); + public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters + // Photonvision thing // 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 final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } + public static final class FieldConstants { + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); + public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); + + + + // Test april tag field layout + // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + // Arrays.asList(new AprilTag[] { + // new AprilTag(1, new Pose3d( + // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + // )), + // }), 100, 100); + + } + public static final class DriveConstants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8b6a0ba..e211436 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,13 +39,16 @@ import com.pathplanner.lib.commands.PathPlannerAuto; // Subsystems // import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Vision; +import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Endeffector; import frc4388.robot.subsystems.SwerveDrive; // Utilites import frc4388.utility.DeferredBlock; +import frc4388.utility.ReefPositionHelper; import frc4388.utility.Subsystem; +import frc4388.utility.ReefPositionHelper.Side; import frc4388.utility.configurable.ConfigurableString; /** @@ -62,7 +65,7 @@ public class RobotContainer { /* Subsystems */ // private final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.camera); - + public final Lidar m_lidar = new Lidar(); public final Elevator m_robotELevator= new Elevator(m_robotMap.elevator); public final Endeffector m_robotEndeffector = new Endeffector(m_robotMap.endeffector); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); @@ -94,6 +97,7 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + configureButtonBindings(); configureVirtualButtonBindings(); new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); @@ -157,7 +161,7 @@ public class RobotContainer { DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - + // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); @@ -174,7 +178,7 @@ public class RobotContainer { // ? /* Operator Buttons */ new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos)); + .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision)); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); @@ -267,7 +271,7 @@ public class RobotContainer { // Load the path you want to follow using its name in the GUI return new PathPlannerAuto("New Auto"); } catch (Exception e) { - DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); + DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); return Commands.none(); } // zach told me to do the below comment diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java index e7d82ed..1d5755b 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -9,6 +9,8 @@ import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Vision; import frc4388.utility.Gains; +import frc4388.utility.ReefPositionHelper; +import frc4388.utility.ReefPositionHelper.Side; import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; import frc4388.utility.UtilityStructs.AutoRecordingFrame; import frc4388.utility.controller.VirtualController; @@ -32,10 +34,9 @@ public class GotoPositionCommand extends Command { * @param SwerveDrive m_robotSwerveDrive */ - public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) { + public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) { this.swerveDrive = swerveDrive; this.vision = vision; - this.targetpos = targetpos; addRequirements(swerveDrive); } @@ -43,6 +44,7 @@ public class GotoPositionCommand extends Command { public void initialize() { xPID.initialize(); yPID.initialize(); + this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.RIGHT, AutoConstants.X_OFFSET_TRIM.get()); } double xerr; diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java new file mode 100644 index 0000000..130efc3 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -0,0 +1,80 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.Counter; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; +import frc4388.utility.Status.ReportLevel; + +// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface +public class Lidar extends Subsystem { + + private double distance = -1; + Counter LidarPWM = new Counter(AutoConstants.LIDAR_DIO_CHANNEL); + + public Lidar() { + LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured + LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement + LidarPWM.reset(); + } + + @Override + public void periodic() { + if(LidarPWM.get() < 1) + distance = -1; + else + distance = (LidarPWM.getPeriod() * AutoConstants.SECONDS_TO_MICROS) / AutoConstants.LIDAR_MICROS_TO_CM; + } + + public double getDistance(){ + return distance; + } + + public boolean withinDistance(){ + if(distance == -1) return false; + return distance < AutoConstants.LIDAR_DETECT_DISTANCE; + } + + ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); + + GenericEntry sbDistance = subsystemLayout + .add("Distance", 0) + .withWidget(BuiltInWidgets.kGraph) + .getEntry(); + + GenericEntry sbWithinDistance = subsystemLayout + .add("Within Distance", 0) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); + + @Override + public String getSubsystemName() { + return "Lidar"; + } + + @Override + public void queryStatus() { + sbDistance.setDouble(distance); + sbWithinDistance.setBoolean(withinDistance()); + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + if(distance == -1){ + s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED"); + }else{ + s.addReport(ReportLevel.ERROR, "LIDAR CONNECTED"); + } + return s; + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 65db294..f0410e9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -43,292 +43,314 @@ public class SwerveDrive extends Subsystem { private SwerveDrivetrain swerveDriveTrain; private Vision vision; - + private int gear_index = SwerveDriveConstants.STARTING_GEAR; private boolean stopped = false; public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25% - + public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to + // 25% + public double rotTarget = 0.0; public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { - // public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { - super(); + // public SwerveDrive(SwerveDrivetrain + // swerveDriveTrain) { + super(); - this.swerveDriveTrain = swerveDriveTrain; - this.vision = vision; + this.swerveDriveTrain = swerveDriveTrain; + this.vision = vision; - RobotConfig config; - try{ - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - config = null; - } + RobotConfig config; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + config = null; + } // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( - () -> { + () -> { var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()); // pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1)); - return pose;//getRotation().times(-1) - }, // Robot pose supplier - swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) - () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() - .withSpeeds(speeds) - ), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants - ), - config, // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + return pose;// getRotation().times(-1) + }, // Robot pose supplier + swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting + // pose) + () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. + // Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for + // holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red + // alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); - } + } - // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ - // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); - // // rightStick.getAngle() - // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); - // // System.out.println(ang); - // // module.go(ang); - // // Rotation2d rot = Rotation2d.fromRadians(ang); - // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); - // SwerveModuleState state = new SwerveModuleState(speed, rot); - // module.setDesiredState(state); + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, + // Translation2d rightStick){ + // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); + // // rightStick.getAngle() + // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + + // Math.pow(leftStick.getY(), 2)); + // // System.out.println(ang); + // // module.go(ang); + // // Rotation2d rot = Rotation2d.fromRadians(ang); + // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); + // SwerveModuleState state = new SwerveModuleState(speed, rot); + // module.setDesiredState(state); // } public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - stopModules(); // stop the swerve - - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput - return; // don't bother doing swerve drive math and return early. + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + stopModules(); // stop the swerve - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - if (fieldRelative) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(-leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getX()*rotSpeedAdjust) - ); - // double rot = 0; - - // ! drift correction - // dependant on if the new odomitry system acounts for rotational drift, this may not be needed. - // if (rightStick.getNorm() > 0.05) { - // rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees(); - // swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - // .withVelocityX(leftStick.getX()*speedAdjust) - // .withVelocityY(leftStick.getY()*speedAdjust) - // .withRotationalRate(rightStick.getY()*rotSpeedAdjust) - // ); - - // // SmartDashboard.putBoolean("drift correction", false); - // stopped = false; - // } else if(leftStick.getNorm() > 0.05) { - // if (!stopped) { - // stopModules(); - // stopped = true; - // } + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput + return; // don't bother doing swerve drive math and return early. - // // SmartDashboard.putBoolean("drift correction", true); - - // // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + if (SwerveDriveConstants.INVERT_X) leftStick = new Translation2d(-leftStick.getX(), leftStick.getY()); + if (SwerveDriveConstants.INVERT_Y) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY()); + if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1); + stopped = false; + if (fieldRelative) { + // ! drift correction + if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) { + rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); + swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + SmartDashboard.putBoolean("drift correction", false); + } else { + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD + ); + swerveDriveTrain.setControl(ctrl); + SmartDashboard.putBoolean("drift correction", true); + } - // } + // // SmartDashboard.putBoolean("drift correction", true); - // // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - // Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - // // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); + // // rot = ((rotTarget - gyro.getAngle()) / 360) * + // SwerveDriveConstants.ROT_CORRECTION_SPEED; - // // Convert field-relative speeds to robot-relative speeds. - // // chassisSpeeds = chassisSpeeds. - // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); - } else { // Create robot-relative speeds. - swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(-leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getX()*rotSpeedAdjust) - ); - } + // } + + // // Use the left joystick to set speed. Apply a cubic curve and the set max + // speed. + // Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + // // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), + // Math.pow(speed.getY(), 3.00)); + + // // Convert field-relative speeds to robot-relative speeds. + // // chassisSpeeds = chassisSpeeds. + // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * + // speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, + // gyro.getRotation2d().times(-1)); + } else { // Create robot-relative speeds. + swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(-leftStick.getY() * speedAdjust) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + } } - public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - stopModules(); // stop the swerve - - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput - return; // don't bother doing swerve drive math and return early. + public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical + // reason to have a robot + // relitive version of + // this, and no pre + // provided version + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve + // drive is still going: + stopModules(); // stop the swerve - leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX()*speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withTargetDirection(rightStick.getAngle()) - ); + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput + return; // don't bother doing swerve drive math and return early. + + leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(rightStick.getAngle())); } public boolean rotateToTarget(double angle) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(0) - .withVelocityY(0) - .withTargetDirection(Rotation2d.fromDegrees(angle)) - ); + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(0) + .withVelocityY(0) + .withTargetDirection(Rotation2d.fromDegrees(angle))); - if (Math.abs(angle - getGyroAngle()) < 5.0) { - return true; - } + if (Math.abs(angle - getGyroAngle()) < 5.0) { + return true; + } - return false; + return false; } public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { - // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - // stopModules(); // stop the swerve - - // if (leftStick.getNorm() < 0.05) //if no imput - // return; // don't bother doing swerve drive math and return early. + // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the + // swerve drive is still going: + // stopModules(); // stop the swerve - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX()*-speedAdjust) - .withVelocityY(leftStick.getY()*speedAdjust) - .withTargetDirection(rot) - ); - // double + // if (leftStick.getNorm() < 0.05) //if no imput + // return; // don't bother doing swerve drive math and return early. + + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * -speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(rot)); + // double } public double getGyroAngle() { - return swerveDriveTrain.getRotation3d().getAngle(); + return swerveDriveTrain.getRotation3d().getAngle(); } public void resetGyro() { - swerveDriveTrain.tareEverything(); + swerveDriveTrain.tareEverything(); } public void stopModules() { - swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + stopped = true; + swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); } @Override public void periodic() { - // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); - SmartDashboard.putNumber("RotTartget", rotTarget); + // This method will be called once per scheduler run\ + SmartDashboard.putNumber("Gyro", getGyroAngle()); + SmartDashboard.putNumber("RotTartget", rotTarget); - double time = Vision.getTime(); + double time = Vision.getTime(); - vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); + vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); - if(vision.isTag()){ - swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); - } + if (vision.isTag()) { + swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); + } - // if(e.isPresent()) + // if(e.isPresent()) } private void reset_index() { - gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?) + gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the + // robot start in?) } public void shiftDown() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index - 1; - if (i == -1) i = 0; - setPercentOutput(SwerveDriveConstants.GEARS[i]); - gear_index = i; + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) + reset_index(); // If outof bounds: reset index + int i = gear_index - 1; + if (i == -1) + i = 0; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; } public void shiftUp() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index + 1; - if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; - setPercentOutput(SwerveDriveConstants.GEARS[i]); - gear_index = i; + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) + reset_index(); // If outof bounds: reset index + int i = gear_index + 1; + if (i == SwerveDriveConstants.GEARS.length) + i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); + gear_index = i; } public void setPercentOutput(double speed) { - speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; - gear_index = -1; + speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + gear_index = -1; } public void setToSlow() { - setPercentOutput(SwerveDriveConstants.SLOW_SPEED); - gear_index = 0; + setPercentOutput(SwerveDriveConstants.SLOW_SPEED); + gear_index = 0; } public void setToFast() { - setPercentOutput(SwerveDriveConstants.FAST_SPEED); - gear_index = 1; + setPercentOutput(SwerveDriveConstants.FAST_SPEED); + gear_index = 1; } public void setToTurbo() { - setPercentOutput(SwerveDriveConstants.TURBO_SPEED); - gear_index = 2; + setPercentOutput(SwerveDriveConstants.TURBO_SPEED); + gear_index = 2; } public void shiftUpRot() { - rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; } public void shiftDownRot() { - rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; } @Override public String getSubsystemName() { - return "Swerve Drive Controller"; + return "Swerve Drive Controller"; } ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); GenericEntry sbGyro = subsystemLayout - .add("Gyro angle", 0) - .withWidget(BuiltInWidgets.kGyro) - .getEntry(); + .add("Gyro angle", 0) + .withWidget(BuiltInWidgets.kGyro) + .getEntry(); GenericEntry sbShiftState = subsystemLayout - .add("Shift State", 0) - .withWidget(BuiltInWidgets.kNumberBar) - .getEntry(); + .add("Shift State", 0) + .withWidget(BuiltInWidgets.kNumberBar) + .getEntry(); @Override public void queryStatus() { - sbGyro.setDouble(getGyroAngle()); - sbShiftState.setDouble(this.speedAdjust); - - //TODO: Add more status things + sbGyro.setDouble(getGyroAngle()); + sbShiftState.setDouble(this.speedAdjust); + + // TODO: Add more status things } @Override public Status diagnosticStatus() { - Status status = new Status(); + Status status = new Status(); - status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); - - return status; + status.addReport(ReportLevel.ERROR, + "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); + + return status; } } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 07ca483..aeed160 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -3,6 +3,7 @@ package frc4388.robot.subsystems; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; @@ -19,6 +20,7 @@ import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import com.ctre.phoenix6.Utils; @@ -30,6 +32,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.Constants.FieldConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.utility.Status; import frc4388.utility.Subsystem; @@ -38,7 +41,8 @@ public class Vision extends Subsystem { private PhotonCamera camera; - private boolean isTag = false; + private boolean isTagDetected = false; + private boolean isTagProcessed = false; private Pose2d lastVisionPose = new Pose2d(); private Pose2d lastPhysOdomPose = new Pose2d(); @@ -52,10 +56,15 @@ public class Vision extends Subsystem { .getLayout(getSubsystemName(), BuiltInLayouts.kList) .withSize(2, 2); - GenericEntry sbTag = subsystemLayout + GenericEntry sbTagDetected = subsystemLayout .add("Tag Detected", false) .withWidget(BuiltInWidgets.kBooleanBox) .getEntry(); + + GenericEntry sbTagProcessed = subsystemLayout + .add("Tag Processed", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); GenericEntry sbCamConnected = subsystemLayout .add("Camera Connnected", false) @@ -66,47 +75,39 @@ public class Vision extends Subsystem { this.camera = camera; SmartDashboard.putData(field); - photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); + photonEstimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } @Override public void periodic() { - var result = camera.getLatestResult(); - isTag = result.hasTargets(); + // var result = camera.getLatestResult(); + var results = camera.getAllUnreadResults(); + if (results.size() == 0) return; + var result = results.get(results.size()-1); + + isTagDetected = result.hasTargets(); + isTagProcessed = false; - // Optional multitag = result.getMultiTagResult(); - // if (multitag.isEmpty()) { - // Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best; - // }else if() - - - // sbTag.setBoolean(isTag); - // field.setRobotPose(getPose2d()); - - // sbCamConnected.setBoolean(camera); - - // System.out.println(isTag); - - if(!isTag){ - sbTag.setBoolean(isTag); + if(!isTagDetected){ + // sbTagDetected.setBoolean(isTagDetected); field.setRobotPose(getPose2d()); return; } - var EstimatedRobotPose = getEstimatedGlobalPose(); + var EstimatedRobotPose = getEstimatedGlobalPose(result); // In case the pose estimator fails to estimate the pose, fallback to physical odometry. if(EstimatedRobotPose.isEmpty()){ - isTag = false; - sbTag.setBoolean(isTag); field.setRobotPose(getPose2d()); return; } + isTagProcessed = true; + lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); - // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation())); + // lastVisionPose.rotateBy(Rotation2d.k180deg); // lastVisionPose = new Pose2d( // lastVisionPose.getTranslation(), // lastPhysOdomPose.getRotation() @@ -130,9 +131,22 @@ public class Vision extends Subsystem { * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets * used for estimation. */ - public Optional getEstimatedGlobalPose() { + public Optional getEstimatedGlobalPose(PhotonPipelineResult change) { Optional visionEst = Optional.empty(); - for (var change : camera.getAllUnreadResults()) { + // for (var change : camera.getAllUnreadResults()) { + + var targets = change.getTargets(); + for(int i = targets.size()-1; i >= 0; i--){ + Transform3d pos = targets.get(i).getBestCameraToTarget(); + double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); + if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { + change.targets.remove(i); + } + } + + if(targets.size() <= 0) + return visionEst; // Will be empty + visionEst = photonEstimator.update(change); updateEstimationStdDevs(visionEst, change.getTargets()); @@ -146,16 +160,11 @@ public class Vision extends Subsystem { // getSimDebugField().getObject("VisionEstimation").setPoses(); // }); // } - } + // } return visionEst; } - - - - - /** * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard * deviations based on number of tags, estimation strategy, and distance from the tags. @@ -179,13 +188,17 @@ public class Vision extends Subsystem { for (var tgt : targets) { var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); if (tagPose.isEmpty()) continue; - numTags++; - avgDist += - tagPose - .get() - .toPose2d() - .getTranslation() - .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + + double distance = tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + + if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { + numTags++; + avgDist += distance; + } } if (numTags == 0) { @@ -229,7 +242,7 @@ public class Vision extends Subsystem { } public Pose2d getPose2d() { - if(isTag) + if(isTagDetected && isTagProcessed) return lastVisionPose; else return lastPhysOdomPose; @@ -240,7 +253,7 @@ public class Vision extends Subsystem { } public boolean isTag(){ - return isTag; + return isTagDetected && isTagProcessed; } @@ -266,7 +279,8 @@ public class Vision extends Subsystem { @Override public void queryStatus() { - sbTag.setBoolean(isTag); + sbTagDetected.setBoolean(isTagDetected); + sbTagProcessed.setBoolean(isTagProcessed); sbCamConnected.setBoolean(camera.isConnected()); // field.setRobotPose(getPose2d()); } diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java new file mode 100644 index 0000000..47a7991 --- /dev/null +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -0,0 +1,95 @@ +package frc4388.utility; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc4388.robot.Constants.FieldConstants; + +public class ReefPositionHelper { + public enum Side { + LEFT, + RIGHT + } + + public static final Pose2d[] RED_TAGS = { + FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(11).get().toPose2d() + }; + + public static final Pose2d[] BLUE_TAGS = { + FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(22).get().toPose2d() + }; + + public static double distanceTo(Pose2d first, Pose2d second){ + return Math.sqrt(Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2)); + } + + + /* + * Function to loop through a list of tag locations to figure out closest one + */ + public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){ + if(locations.length <= 0) return new Pose2d(); + + Pose2d minPos = locations[0]; + double minDistance = distanceTo(position,minPos); + + for(int i = 1; i < locations.length; i++){ + double dist = distanceTo(locations[i],position); + if(dist < minDistance){ + System.out.println(i); + minPos = locations[i]; + minDistance = dist; + } + } + + System.out.println(minPos); + + return minPos; + } + + /* + * Function to find closest tag location based on side + */ + public static Pose2d getNearestTag(Pose2d position) { + Optional ally = DriverStation.getAlliance(); + if (!ally.isPresent()) + return new Pose2d(); + if (ally.get() == Alliance.Red) + return getNearestTag(RED_TAGS, position); + if (ally.get() == Alliance.Blue) + return getNearestTag(BLUE_TAGS, position); + return new Pose2d(); + } + + public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim) { + return offset(getNearestTag(position), + (side == Side.LEFT ? -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)) + xtrim, + FieldConstants.VERTICAL_SCORING_POSITION_OFFSET); + } + + + public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){ + Translation2d oldTranslation = oldPose.getTranslation(); + + double rot = oldPose.getRotation().getRadians(); + + return new Pose2d(new Translation2d( + oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset, + oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset + ), oldPose.getRotation()); + } +}