diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 610f140..0a31049 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,9 +15,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3608ad8..aeb3808 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -55,11 +55,10 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import frc4388.robot.subsystems.Elevator; // Subsystems import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Elevator.CoordinationState; // import frc4388.robot.subsystems.Endeffector; import frc4388.robot.subsystems.SwerveDrive; - +import frc4388.robot.subsystems.vision.Vision; // Utilites import frc4388.utility.DeferredBlock; import frc4388.utility.compute.TimesNegativeOne; @@ -695,8 +694,8 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) - .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust *= 2;})) - .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust /= 2;})); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;})) + .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;})); new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod())) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 00be570..1e0cb87 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -21,9 +21,12 @@ import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.LiDARConstants; import frc4388.robot.constants.Constants.VisionConstants; import frc4388.robot.constants.DriveConstants; -import frc4388.robot.subsystems.Lidar; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.lidar.LidarIO; +import frc4388.robot.subsystems.lidar.LidarLiteV2; +import frc4388.robot.subsystems.vision.VisionIO; +import frc4388.robot.subsystems.vision.VisionPhotonvision; import frc4388.utility.status.FaultCANCoder; -import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; import frc4388.utility.status.FaultTalonFX; @@ -35,11 +38,13 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public final PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); - public final PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + public final VisionIO leftCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME), VisionConstants.LEFT_CAMERA_POS); + public final VisionIO rightCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME), VisionConstants.RIGHT_CAMERA_POS); - public final Lidar reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef"); - public final Lidar reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse"); + // public final LiDAR lidar = new + + public final LiDAR reefLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL), "Reef"); + public final LiDAR reverseLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL), "Reverse"); /* LED Subsystem */ @@ -69,8 +74,8 @@ public class RobotMap { public RobotMap() { configureDriveMotorControllers(); - FaultPhotonCamera.addDevice(leftCamera, "Left Camera"); - FaultPhotonCamera.addDevice(rightCamera, "Right Camera"); + // FaultPhotonCamera.addDevice(leftCamera, "Left Camera"); + // FaultPhotonCamera.addDevice(rightCamera, "Right Camera"); FaultPidgeon2.addDevice(swerveDrivetrain.getPigeon2(), "Gyro"); @@ -111,8 +116,8 @@ public class RobotMap { cameraProp.setAvgLatencyMs(35); cameraProp.setLatencyStdDevMs(5); - sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); - sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); + // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); + // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); sim.leftCamera.enableRawStream(true); diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java index 11a1f82..3d13052 100644 --- a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.subsystems.Vision; +import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.ReefPositionHelper; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.compute.ReefPositionHelper.Side; @@ -131,7 +131,7 @@ public class DriveToReef extends Command { (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && - (!waitVelocityZero || swerveDrive.state.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) + (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) ); // System.out.println(finished); diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java index b3b35a4..d6ca489 100644 --- a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java @@ -2,22 +2,22 @@ package frc4388.robot.commands.alignment; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.lidar.LiDAR; // Command to repeat a joystick movement for a specific time. public class DriveUntilLiDAR extends Command { private final SwerveDrive swerveDrive; private final Translation2d leftStick; private final Translation2d rightStick; - private final Lidar m_lidar; + private final LiDAR m_lidar; private final double mindistance; public DriveUntilLiDAR( SwerveDrive swerveDrive, Translation2d leftStick, Translation2d rightStick, - Lidar lidar, + LiDAR lidar, double mindistance) { addRequirements(swerveDrive); diff --git a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java index c40ca26..668d3fa 100644 --- a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java @@ -8,13 +8,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.lidar.LiDAR; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class LidarAlign extends Command { private SwerveDrive swerveDrive; - private Lidar lidar; + private LiDAR lidar; private int currentFinderTick; // private int tickFoundPipe; @@ -26,7 +26,7 @@ public class LidarAlign extends Command { // private final boolean constructedHeadedRight; /** Creates a new LidarAlign. */ - public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) { + public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) { // Use addRequirements() here to declare subsystem dependencies. this.swerveDrive = swerveDrive; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index ae418b7..f131ea0 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 170; - public static final String GIT_SHA = "2a38f94d5eef00a093f47df192f7c5c8a2b8cf8d"; - public static final String GIT_DATE = "2025-07-15 10:24:11 MDT"; + public static final int GIT_REVISION = 171; + public static final String GIT_SHA = "3130f647c83cc82b45a5299e19108f9eec45e6f6"; + public static final String GIT_DATE = "2025-07-15 11:07:01 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-15 11:04:29 MDT"; - public static final long BUILD_UNIX_TIME = 1752599069523L; + public static final String BUILD_DATE = "2025-07-15 13:40:35 MDT"; + public static final long BUILD_UNIX_TIME = 1752608435113L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 5c17506..7ac9418 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -4,8 +4,6 @@ package frc4388.robot.subsystems; -import org.littletonrobotics.junction.AutoLog; - import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 1e86ae2..c050278 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -11,7 +11,6 @@ import org.littletonrobotics.junction.AutoLogOutput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.LEDConstants; import frc4388.utility.status.Status; diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java deleted file mode 100644 index 37ec4de..0000000 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ /dev/null @@ -1,71 +0,0 @@ -package frc4388.robot.subsystems; - -import org.littletonrobotics.junction.AutoLogOutput; - -import edu.wpi.first.networktables.GenericEntry; -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 edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.constants.Constants.LiDARConstants; -import frc4388.utility.status.Status; -import frc4388.utility.status.FaultReporter; -import frc4388.utility.status.Queryable; -import frc4388.utility.status.Status.ReportLevel; - -// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface -public class Lidar extends SubsystemBase implements Queryable { - - private Counter LidarPWM; - private String name = "Lidar"; - - private double distance = -1; - public Lidar(int port, String name) { - FaultReporter.register(this); - - this.name = name; - LidarPWM = new Counter(port); - 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() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM; - } - - @AutoLogOutput(key = "Lidar/{name}") - public double getDistance(){ - return distance; - } - - public boolean withinDistance(){ - if(distance == -1) return false; - return distance < LiDARConstants.LIDAR_DETECT_DISTANCE; - } - - @Override - public String getName() { - return "Lidar " + name; - } - - @Override - public Status diagnosticStatus() { - Status s = new Status(); - - if(distance == -1) - s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED"); - - s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance); - - return s; - } - -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 17bf2dd..e37de34 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -6,7 +6,6 @@ package frc4388.robot.subsystems; import java.util.Optional; -import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; @@ -20,15 +19,11 @@ 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.math.kinematics.ChassisSpeeds; -import edu.wpi.first.networktables.GenericEntry; -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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.DriveConstants; +import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.status.Status; import frc4388.utility.status.FaultReporter; @@ -44,28 +39,28 @@ public class SwerveDrive extends SubsystemBase implements Queryable { private SwerveDrivetrain swerveDriveTrain; private Vision vision; - @AutoLog - public class SwerveDriveState { - public int gear_index = DriveConstants.STARTING_GEAR; - public boolean stopped = false; - public boolean robotKnowsWhereItIs = false; - - public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index]; - public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to - // 25% - - public double lastOdomSpeed; - - public Pose2d initalPose2d = null; - - - public double rotTarget = 0.0; - public Rotation2d orientRotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - } + // @AutoLog + // public class SwerveDriveState { + public int gear_index = DriveConstants.STARTING_GEAR; + public boolean stopped = false; + public boolean robotKnowsWhereItIs = false; - public SwerveDriveState state = new SwerveDriveState(); + public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index]; + public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED; + public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to + // 25% + + public double lastOdomSpeed; + + public Pose2d initalPose2d = null; + + + public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + // } + + // public SwerveDriveState state = new SwerveDriveState(); /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { @@ -86,7 +81,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { - return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(state.initalPose2d); + return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); }, // Robot pose supplier this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting // pose) @@ -135,7 +130,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // null, // null, // null, - // (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + // (state) -> Logger.recordOutput("Drive/SysIdState", toString())), // new SysIdRoutine.Mechanism( // (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); @@ -143,7 +138,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void setOdoPose(Pose2d pose) { if (pose == null) return; - state.initalPose2d = pose; + initalPose2d = pose; swerveDriveTrain.resetPose(pose); } @@ -162,7 +157,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // } public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve drive is still going: + 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 @@ -170,7 +165,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - state.stopped = false; + stopped = false; if (fieldRelative) { leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); @@ -178,18 +173,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // ! drift correction if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) { - state.rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); + rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) - .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); SmartDashboard.putBoolean("drift correction", false); } else { var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) - .withTargetDirection(Rotation2d.fromDegrees(state.rotTarget)); + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); ctrl.HeadingController.setPID( DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, @@ -202,20 +197,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } else { // Create robot-relative speeds. swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(-leftStick.getY() * state.speedAdjust) - .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(-leftStick.getY() * speedAdjust) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); } } public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) { - state.stopped = false; + stopped = false; // Create robot-relative speeds. if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) - .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); } @@ -225,7 +220,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // relitive version of // this, and no pre // provided version - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve + 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 @@ -235,8 +230,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rightStick.getAngle())); } @@ -244,8 +239,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); ctrl.HeadingController.setPID( DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, @@ -259,8 +254,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(heading); var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); // ctrl.HeadingController.setPID( // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, @@ -304,7 +299,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public boolean isStopped() { - return state.lastOdomSpeed < AutoConstants.STOP_VELOCITY; + return lastOdomSpeed < AutoConstants.STOP_VELOCITY; } public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { @@ -318,8 +313,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * -state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) + .withVelocityX(leftStick.getX() * -speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rot)); // double } @@ -329,19 +324,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public Pose2d getPose2d() { - return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(state.initalPose2d); + return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d); } public void resetGyro() { swerveDriveTrain.tareEverything(); - state.robotKnowsWhereItIs = false; - state.rotTarget = 0; + robotKnowsWhereItIs = false; + rotTarget = 0; // vision.resetRotations(); } public void softStop() { - state.stopped = true; + stopped = true; swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() .withVelocityX(0) .withVelocityY(0) @@ -359,7 +354,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void periodic() { // This method will be called once per scheduler run\ SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI); - SmartDashboard.putNumber("RotTartget", state.rotTarget); + SmartDashboard.putNumber("RotTartget", rotTarget); double time = Vision.getTime(); double freq = swerveDriveTrain.getOdometryFrequency(); @@ -370,86 +365,95 @@ public class SwerveDrive extends SubsystemBase implements Queryable { vision.setLastOdomPose(curpose); setLastOdomSpeed(curpose, lastPose, freq); - // if (vision.isTag()) {5 + if (vision.isTag()) { + Pose2d pose = vision.getPose2d(); + if (!robotKnowsWhereItIs) { + robotKnowsWhereItIs = true; + Pose2d curPose = getPose2d(); + rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees(); + } + + vision.addVisionMeasurement(swerveDriveTrain); + } // if(e.isPresent()) } private void reset_index() { - state.gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the + gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the // robot start in?) } public void shiftDown() { - if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length) + if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = state.gear_index - 1; + int i = gear_index - 1; if (i == -1) i = 0; setPercentOutput(DriveConstants.GEARS[i]); - state.gear_index = i; + gear_index = i; } public void shiftUp() { - if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length) + if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = state.gear_index + 1; + int i = gear_index + 1; if (i == DriveConstants.GEARS.length) i = DriveConstants.GEARS.length - 1; setPercentOutput(DriveConstants.GEARS[i]); - state.gear_index = i; + gear_index = i; } public void setPercentOutput(double speed) { - state.speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; - state.gear_index = -1; + speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + gear_index = -1; } public void setToSlow() { setPercentOutput(DriveConstants.SLOW_SPEED); - state.gear_index = 0; + gear_index = 0; } public void setToFast() { setPercentOutput(DriveConstants.FAST_SPEED); - state.gear_index = 1; + gear_index = 1; } public void setToTurbo() { setPercentOutput(DriveConstants.TURBO_SPEED); - state.gear_index = 2; + gear_index = 2; } public void shiftUpRot() { - state.rotSpeedAdjust = DriveConstants.ROTATION_SPEED; + rotSpeedAdjust = DriveConstants.ROTATION_SPEED; } public void shiftDownRot() { - state.rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; + rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; } private int tmp_gear_index = DriveConstants.STARTING_GEAR; public void startSlowPeriod() { - tmp_gear_index = state.gear_index; + tmp_gear_index = gear_index; setToSlow(); } public void startTurboPeriod() { - tmp_gear_index = state.gear_index; + tmp_gear_index = gear_index; setToTurbo(); } public void endSlowPeriod() { setPercentOutput(DriveConstants.GEARS[tmp_gear_index]); - state.gear_index = tmp_gear_index; + gear_index = tmp_gear_index; } public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ if(curPose.isPresent() && lastPose.isPresent()){ - state.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; + lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; } } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java deleted file mode 100644 index 96753a0..0000000 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ /dev/null @@ -1,281 +0,0 @@ -package frc4388.robot.subsystems; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; - -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; - -import org.littletonrobotics.junction.AutoLog; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; - -import edu.wpi.first.networktables.GenericEntry; -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 edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.constants.Constants.FieldConstants; -import frc4388.robot.constants.Constants.VisionConstants; -import frc4388.utility.status.Status; -import frc4388.utility.status.FaultReporter; -import frc4388.utility.status.Queryable; - -public class Vision extends SubsystemBase implements Queryable { - private PhotonCamera[] cameras; - private PhotonPoseEstimator[] estimators; - - @AutoLog - public class VisionState { - public boolean isTagDetected = false; - public boolean isTagProcessed = false; - public List poses = new ArrayList<>(); - public double latency = 0; - public Pose2d lastVisionPose = new Pose2d(); - public Pose2d lastPhysOdomPose = new Pose2d(); - } - - private VisionState state = new VisionState(); - - private Field2d field = new Field2d(); - public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){ - FaultReporter.register(this); - SmartDashboard.putData(field); - - this.cameras = new PhotonCamera[]{leftCamera, rightCamera}; - - PhotonPoseEstimator photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS); - PhotonPoseEstimator photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS); - - photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - - this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight}; - } - - @Override - public void periodic() { - update(); - field.setRobotPose(getPose2d()); - } - - // private Instant lastVisionTime = null; - - - private void update() { - state.isTagProcessed = false; - state.isTagDetected = false; - - // Instant now = Instant.now(); - - // int cams = 0; - - double latency = 0; - - // Pose2d pose = null; - state.poses.clear(); - - for(int i = 0; i < cameras.length; i++){ - PhotonCamera camera = cameras[i]; - PhotonPoseEstimator estimator = estimators[i]; - - var results = camera.getAllUnreadResults(); - - // If there are no more updates from the camera - if (results.size() == 0) - continue; - - - var result = results.get(results.size()-1); - latency += result.getTimestampSeconds(); - - state.isTagDetected = state.isTagDetected | result.hasTargets(); - - // If there are no tags - if(!result.hasTargets()) - continue; - - Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); - - // If the tag was failed to be processed - if(estimatedRobotPose.isEmpty()) - continue; - - state.poses.add(estimatedRobotPose.get()); - - // if(pose == null) - // pose = estimatedRobotPose.get().estimatedPose.toPose2d(); - // else - // pose = pose.interpolate(pose, 0.5); - // X += pose.getX(); - // Y += pose.getY(); - - // if(X > 6) - - // Yaw += (pose.getRotation().getDegrees() + 180) % 360; - // cams++; - - state.isTagProcessed = true; - - - } - } - - - /** - * The latest estimated robot pose on the field from vision data. This may be empty. This should - * only be called once per loop. - * - *

Also includes updates for the standard deviations, which can (optionally) be retrieved with - * {@link getEstimationStdDevs} - * - * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets - * used for estimation. - */ - public Optional getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) { - Optional visionEst = Optional.empty(); - - 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 = estimator.update(change); - // updateEstimationStdDevs(visionEst, change.getTargets(), estimator); - - 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. - // * - // * @param estimatedPose The estimated pose to guess standard deviations for. - // * @param targets All targets in this camera frame - // */ - // private void updateEstimationStdDevs( - // Optional estimatedPose, - // List targets, - // PhotonPoseEstimator estimator) { - // if (estimatedPose.isEmpty()) { - // // No pose input. Default to single-tag std devs - // curStdDevs = VisionConstants.kSingleTagStdDevs; - - // } else { - // // Pose present. Start running Heuristic - // var estStdDevs = VisionConstants.kSingleTagStdDevs; - // int numTags = 0; - // double avgDist = 0; - - // // Precalculation - see how many tags we found, and calculate an average-distance metric - // for (var tgt : targets) { - // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); - // if (tagPose.isEmpty()) continue; - - // double distance = tagPose - // .get() - // .toPose2d() - // .getTranslation() - // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); - - // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { - // numTags++; - // avgDist += distance; - // } - // } - - // if (numTags == 0) { - // // No tags visible. Default to single-tag std devs - // curStdDevs = 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 = 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); - // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); - // curStdDevs = estStdDevs; - // } - // } - // } - - - - - - public void setLastOdomPose(Optional pose){ - if(pose.isPresent()) - state.lastPhysOdomPose = pose.get(); - } - - // public double getLastOdomSpeed(){ - // return lastOdomSpeed; - // } - - public Pose2d getPose2d() { - if(state.lastPhysOdomPose != null) - return state.lastPhysOdomPose; - - // if(lastVisionPose != null) - // return lastVisionPose; - return new Pose2d(); - - } - - public static double getTime() { - return Utils.getCurrentTimeSeconds(); - } - - public boolean isTag(){ - return state.isTagDetected && state.isTagProcessed; - } - - - public void addVisionMeasurement( SwerveDrivetrain drivetrain){ - for(EstimatedRobotPose pose : state.poses){ - drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds)); - } - } - - - - - - - - - @Override - public String getName() { - return "Vision"; - } - - @Override - public Status diagnosticStatus() { - return new Status(); - } - -} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java new file mode 100644 index 0000000..dbbb024 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java @@ -0,0 +1,55 @@ +package frc4388.robot.subsystems.lidar; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.utility.status.Status; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status.ReportLevel; + +public class LiDAR extends SubsystemBase implements Queryable { + LidarIO io; + LidarStateAutoLogged state = new LidarStateAutoLogged(); + + private String name = "Lidar"; + + public LiDAR(LidarIO device, String name) { + FaultReporter.register(this); + + this.io = device; + this.name = name; + } + + @Override + public void periodic() { + io.updateInputs(state); + } + + // @AutoLogOutput(key = "Lidar/{name}") + public double getDistance(){ + return state.distance; + } + + public boolean withinDistance(){ + if(state.distance == -1) return false; + return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE; + } + + @Override + public String getName() { + return "Lidar " + name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + if(state.distance == -1) + s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED"); + + s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance); + + return s; + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java new file mode 100644 index 0000000..e3b4ebc --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java @@ -0,0 +1,13 @@ +package frc4388.robot.subsystems.lidar; + +import org.littletonrobotics.junction.AutoLog; + +public interface LidarIO { + @AutoLog + public class LidarState { + public boolean connected; + public double distance; + } + + public default void updateInputs(LidarState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java new file mode 100644 index 0000000..3851050 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java @@ -0,0 +1,27 @@ +package frc4388.robot.subsystems.lidar; + +import edu.wpi.first.wpilibj.Counter; +import frc4388.robot.constants.Constants.LiDARConstants; + +// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface +public class LidarLiteV2 implements LidarIO { + + + private Counter LidarPWM; + + public LidarLiteV2(int port) { + LidarPWM = new Counter(port); + 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 updateInputs(LidarState state) { + + if(LidarPWM.get() < 1) + state.distance = -1; + else + state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..d24ee3f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -0,0 +1,94 @@ +package frc4388.robot.subsystems.vision; + +import java.util.Optional; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status; + +public class Vision extends SubsystemBase implements Queryable { + VisionIO[] io; + VisionStateAutoLogged[] state; + + + public Pose2d lastVisionPose = new Pose2d(); + public Pose2d lastPhysOdomPose = new Pose2d(); + + public Vision(VisionIO... devices) { + FaultReporter.register(this); + io = devices; + state = new VisionStateAutoLogged[io.length]; + + for(int i = 0; i < io.length; i++) { + state[i] = new VisionStateAutoLogged(); + } + } + + @Override + public void periodic() { + for(int i = 0; i < io.length; i++) { + io[i].updateInputs(state[i]); + Logger.processInputs("Vision/Camera" + i , state[i]); + } + } + + public void addVisionMeasurement(SwerveDrivetrain drivetrain){ + // for(EstimatedRobotPose pose : poses){ + // + // } + for(int i = 0; i < state.length; i++) { + if(state[i].lastEstimatedPose != null) { + PoseObservation pose = state[i].lastEstimatedPose; + drivetrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp())); + } + } + } + + public void setLastOdomPose(Optional pose){ + if(pose.isPresent()) + lastPhysOdomPose = pose.get(); + } + + public boolean isTag(){ + for(int i = 0; i < state.length; i++){ + if(state[i].isTagDetected && state[i].isTagProcessed) + return true; + } + return false; + } + + @AutoLogOutput + public Pose2d getPose2d() { + if(lastPhysOdomPose != null) + return lastPhysOdomPose; + + // if(lastVisionPose != null) + // return lastVisionPose; + return new Pose2d(); + + } + + public static double getTime() { + return Utils.getCurrentTimeSeconds(); + } + + + @Override + public Status diagnosticStatus() { + return new Status(); + // // TODO Auto-generated method stub + // throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'"); + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java b/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..f979135 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,22 @@ +package frc4388.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose2d; + +public interface VisionIO { + @AutoLog + public class VisionState { + public boolean isTagDetected = false; + public boolean isTagProcessed = false; + // public double latency = 0; + public PoseObservation lastEstimatedPose = null; + } + + public static record PoseObservation( + Pose2d pose, + double timestamp + ) {} + + public default void updateInputs(VisionState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java b/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java new file mode 100644 index 0000000..888dc12 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java @@ -0,0 +1,158 @@ +package frc4388.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Transform3d; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import frc4388.robot.constants.Constants.FieldConstants; +import frc4388.robot.constants.Constants.VisionConstants; + +public class VisionPhotonvision implements VisionIO { + // private PhotonCamera[] cameras; + // private PhotonPoseEstimator[] estimators; + + private PhotonCamera camera; + private PhotonPoseEstimator estimator; + + // public List poses = new ArrayList<>(); + + + public VisionPhotonvision(PhotonCamera camera, Transform3d position){ + this.camera = camera; + estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position); + estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + // private Instant lastVisionTime = null; + + + public void updateInputs(VisionState state) { + state.isTagProcessed = false; + state.isTagDetected = false; + + state.lastEstimatedPose = null; + + var results = camera.getAllUnreadResults(); + + // If there are no more updates from the camera + if (results.size() == 0) + return; + + + var result = results.get(results.size()-1); + + state.isTagDetected = state.isTagDetected | result.hasTargets(); + + // If there are no tags + if(!result.hasTargets()) + return; + + Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); + + // If the tag was failed to be processed + if(estimatedRobotPose.isEmpty()) + return; + + EstimatedRobotPose pose = estimatedRobotPose.get(); + + state.lastEstimatedPose = new PoseObservation(pose.estimatedPose.toPose2d(), pose.timestampSeconds); + + state.isTagProcessed = true; + } + + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should + * only be called once per loop. + * + *

Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link getEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + public Optional getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) { + Optional visionEst = Optional.empty(); + + 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 = estimator.update(change); + // updateEstimationStdDevs(visionEst, change.getTargets(), estimator); + + return visionEst; + } + + public String getName() { + return camera.getName(); + } + + + // /** + // * 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. + // * + // * @param estimatedPose The estimated pose to guess standard deviations for. + // * @param targets All targets in this camera frame + // */ + // private void updateEstimationStdDevs( + // Optional estimatedPose, + // List targets, + // PhotonPoseEstimator estimator) { + // if (estimatedPose.isEmpty()) { + // // No pose input. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; + + // } else { + // // Pose present. Start running Heuristic + // var estStdDevs = VisionConstants.kSingleTagStdDevs; + // int numTags = 0; + // double avgDist = 0; + + // // Precalculation - see how many tags we found, and calculate an average-distance metric + // for (var tgt : targets) { + // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); + // if (tagPose.isEmpty()) continue; + + // double distance = tagPose + // .get() + // .toPose2d() + // .getTranslation() + // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + + // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { + // numTags++; + // avgDist += distance; + // } + // } + + // if (numTags == 0) { + // // No tags visible. Default to single-tag std devs + // curStdDevs = 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 = 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); + // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + // curStdDevs = estStdDevs; + // } + // } + // } +} diff --git a/src/main/java/frc4388/utility/status/Alerts.java b/src/main/java/frc4388/utility/status/Alerts.java index c8e9c1c..4065586 100644 --- a/src/main/java/frc4388/utility/status/Alerts.java +++ b/src/main/java/frc4388/utility/status/Alerts.java @@ -6,9 +6,5 @@ import frc4388.robot.RobotContainer; // Class to update a series of WPILIB Alerts public class Alerts { - private static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError); - - public static void UpdateAlerts(RobotContainer m_RobotContainer) { - no_auto.set(!m_RobotContainer.autoChooserUpdated); - } + public static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError); }