diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 0dfb72d..610f140 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,7 +15,9 @@ 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; @@ -57,6 +59,8 @@ public class Robot extends LoggedRobot { // FaultReporter.startThread(); } + + /** * This function is called every robot packet, no matter the mode. Use * this for items like diagnostics that you want ran during disabled, @@ -77,7 +81,6 @@ public class Robot extends LoggedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); } - /** * This function is called once each time the robot enters Disabled mode. * You can use it to reset any subsystem information you want to clear when @@ -125,6 +128,7 @@ public class Robot extends LoggedRobot { public void autonomousPeriodic() { } + @Override public void teleopInit() { m_robotContainer.stop(); @@ -137,10 +141,7 @@ public class Robot extends LoggedRobot { CommandScheduler.getInstance().cancel(m_autonomousCommand); m_autonomousCommand.cancel(); m_autonomousCommand.end(true); - System.out.println("NOT Null!!"); - } else { - System.out.println("Null!!"); } m_robotTime.startMatchTime(); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 07623b1..3608ad8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -51,11 +51,12 @@ import frc4388.robot.constants.Constants.OIConstants; import com.pathplanner.lib.auto.NamedCommands; 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.Elevator; // import frc4388.robot.subsystems.Endeffector; import frc4388.robot.subsystems.SwerveDrive; @@ -560,7 +561,7 @@ public class RobotContainer { NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE)); // NamedCommands.registerCommand("feed-driveback", Commands.none()); - NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY)); + NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.isStopped())); NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, @@ -694,8 +695,8 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) - .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;})) - .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;})); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust *= 2;})) + .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust /= 2;})); new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod())) @@ -890,6 +891,7 @@ public class RobotContainer { // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); } + public boolean autoChooserUpdated = false; public void makeAutoChooser() { autoChooser = new SendableChooser(); @@ -913,6 +915,7 @@ public class RobotContainer { } autoChooser.onChange((filename) -> { + autoChooserUpdated = true; if (filename.equals("Taxi")) { autoCommand = new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java index ac59724..11a1f82 100644 --- a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java @@ -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.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) + (!waitVelocityZero || swerveDrive.state.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) ); // System.out.println(finished); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 9a50e63..ae418b7 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 = 169; - public static final String GIT_SHA = "aaef829ad2830d39cc9f9e05e61e973658d7784d"; - public static final String GIT_DATE = "2025-07-15 09:33:40 MDT"; + 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 String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-15 09:49:13 MDT"; - public static final long BUILD_UNIX_TIME = 1752594553460L; + public static final String BUILD_DATE = "2025-07-15 11:04:29 MDT"; + public static final long BUILD_UNIX_TIME = 1752599069523L; 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 3157bae..5c17506 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -4,6 +4,8 @@ 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; @@ -25,24 +27,29 @@ public class Elevator extends SubsystemBase implements Queryable { private TalonFX endeffectorMotor; private LED led; - @SuppressWarnings("unused") - private long wait = 0; - private long maxWait = 1000; + // @AutoLog + // private class ElevatorState { + @SuppressWarnings("unused") + public long wait = 0; + public long maxWait = 1000; - private double elevatorRefrence = 0; - private double endeffectorRefrence = 0; + public double elevatorRefrence = 0; + public double endeffectorRefrence = 0; - private boolean elevatorManualStop = true; - private boolean endefectorManualStop = true; + public boolean elevatorManualStop = true; + public boolean endefectorManualStop = true; - private boolean disableAutoIntake = false; + public boolean disableAutoIntake = false; - private boolean seededZeroEndefector = false; - private boolean seededZeroElevator = false; + public boolean seededZeroEndefector = false; + public boolean seededZeroElevator = false; - private DigitalInput basinBeamBreak; - private DigitalInput endeffectorLimitSwitch; - private DigitalInput intakeIR; + public DigitalInput basinBeamBreak; + public DigitalInput endeffectorLimitSwitch; + public DigitalInput intakeIR; + // } + + // private ElevatorState state = new ElevatorState(); public enum CoordinationState { Waiting, // for coral into the though diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index fceffe4..1e86ae2 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -60,11 +60,6 @@ public class LED extends SubsystemBase implements Queryable { return "LEDs"; } - // @Override - // public void queryStatus() { - // SmartDashboard.putString("LED status", mode.name()); - // } - @Override public Status diagnosticStatus() { Status status = new Status(); diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 6981d7a..37ec4de 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -51,10 +51,6 @@ public class Lidar extends SubsystemBase implements Queryable { return distance < LiDARConstants.LIDAR_DETECT_DISTANCE; } - ShuffleboardLayout subsystemLayout; - GenericEntry sbDistance; - GenericEntry sbWithinDistance; - @Override public String getName() { return "Lidar " + name; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 4a11b1b..17bf2dd 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -6,6 +6,7 @@ package frc4388.robot.subsystems; import java.util.Optional; +import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; @@ -26,6 +27,7 @@ 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.utility.compute.TimesNegativeOne; import frc4388.utility.status.Status; @@ -40,27 +42,30 @@ import com.pathplanner.lib.config.RobotConfig; public class SwerveDrive extends SubsystemBase implements Queryable { private SwerveDrivetrain swerveDriveTrain; - private Vision vision; - private int gear_index = DriveConstants.STARTING_GEAR; - private boolean stopped = false; - @SuppressWarnings("unused") - private boolean robotKnowsWhereItIs = false; + @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(); + } - 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) { @@ -81,7 +86,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { - return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); + return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(state.initalPose2d); }, // Robot pose supplier this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting // pose) @@ -138,7 +143,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void setOdoPose(Pose2d pose) { if (pose == null) return; - initalPose2d = pose; + state.initalPose2d = pose; swerveDriveTrain.resetPose(pose); } @@ -157,7 +162,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 && stopped == false) // if no imput and the swerve drive is still going: + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.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 @@ -165,7 +170,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - stopped = false; + state.stopped = false; if (fieldRelative) { leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); @@ -173,18 +178,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // ! drift correction if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) { - rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); + state.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)); + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) + .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); SmartDashboard.putBoolean("drift correction", false); } else { var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) - .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) + .withTargetDirection(Rotation2d.fromDegrees(state.rotTarget)); ctrl.HeadingController.setPID( DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, @@ -197,20 +202,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } else { // Create robot-relative speeds. swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX() * speedAdjust) - .withVelocityY(-leftStick.getY() * speedAdjust) - .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(-leftStick.getY() * state.speedAdjust) + .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); } } public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) { - stopped = false; + state.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() * rotSpeedAdjust)); + .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); } @@ -220,7 +225,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 && stopped == false) // if no imput and the swerve + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve // drive is still going: stopModules(); // stop the swerve @@ -230,8 +235,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) .withTargetDirection(rightStick.getAngle())); } @@ -239,8 +244,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() * speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) .withTargetDirection(heading); ctrl.HeadingController.setPID( DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, @@ -254,8 +259,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(heading); var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) .withTargetDirection(heading); // ctrl.HeadingController.setPID( // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, @@ -298,6 +303,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable { return false; } + public boolean isStopped() { + return state.lastOdomSpeed < AutoConstants.STOP_VELOCITY; + } + public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the // swerve drive is still going: @@ -309,8 +318,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * -speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) + .withVelocityX(leftStick.getX() * -state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) .withTargetDirection(rot)); // double } @@ -320,19 +329,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public Pose2d getPose2d() { - return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d); + return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(state.initalPose2d); } public void resetGyro() { swerveDriveTrain.tareEverything(); - robotKnowsWhereItIs = false; - rotTarget = 0; + state.robotKnowsWhereItIs = false; + state.rotTarget = 0; // vision.resetRotations(); } public void softStop() { - stopped = true; + state.stopped = true; swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() .withVelocityX(0) .withVelocityY(0) @@ -350,7 +359,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", rotTarget); + SmartDashboard.putNumber("RotTartget", state.rotTarget); double time = Vision.getTime(); double freq = swerveDriveTrain.getOdometryFrequency(); @@ -367,82 +376,80 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } private void reset_index() { - gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the + state.gear_index = DriveConstants.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 >= DriveConstants.GEARS.length) + if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index - 1; + int i = state.gear_index - 1; if (i == -1) i = 0; setPercentOutput(DriveConstants.GEARS[i]); - gear_index = i; + state.gear_index = i; } public void shiftUp() { - if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) + if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index + 1; + int i = state.gear_index + 1; if (i == DriveConstants.GEARS.length) i = DriveConstants.GEARS.length - 1; setPercentOutput(DriveConstants.GEARS[i]); - gear_index = i; + state.gear_index = i; } public void setPercentOutput(double speed) { - speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; - gear_index = -1; + state.speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + state.gear_index = -1; } public void setToSlow() { setPercentOutput(DriveConstants.SLOW_SPEED); - gear_index = 0; + state.gear_index = 0; } public void setToFast() { setPercentOutput(DriveConstants.FAST_SPEED); - gear_index = 1; + state.gear_index = 1; } public void setToTurbo() { setPercentOutput(DriveConstants.TURBO_SPEED); - gear_index = 2; + state.gear_index = 2; } public void shiftUpRot() { - rotSpeedAdjust = DriveConstants.ROTATION_SPEED; + state.rotSpeedAdjust = DriveConstants.ROTATION_SPEED; } public void shiftDownRot() { - rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; + state.rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; } private int tmp_gear_index = DriveConstants.STARTING_GEAR; public void startSlowPeriod() { - tmp_gear_index = gear_index; + tmp_gear_index = state.gear_index; setToSlow(); } public void startTurboPeriod() { - tmp_gear_index = gear_index; + tmp_gear_index = state.gear_index; setToTurbo(); } public void endSlowPeriod() { setPercentOutput(DriveConstants.GEARS[tmp_gear_index]); - gear_index = tmp_gear_index; + state.gear_index = tmp_gear_index; } public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ if(curPose.isPresent() && lastPose.isPresent()){ - this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; - - SmartDashboard.putNumber("Speed", lastOdomSpeed); + state.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; } } @@ -453,28 +460,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { return "Swerve Drive Controller"; } - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbGyro = subsystemLayout - .add("Gyro angle", 0) - .withWidget(BuiltInWidgets.kGyro) - .getEntry(); - - GenericEntry sbShiftState = subsystemLayout - .add("Shift State", 0) - .withWidget(BuiltInWidgets.kNumberBar) - .getEntry(); - - // @Override - // public void queryStatus() { - // sbGyro.setDouble(getGyroAngle()); - // sbShiftState.setDouble(this.speedAdjust); - - // // TODO: Add more status things - // } - @Override public Status diagnosticStatus() { Status status = new Status(); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index b11c501..96753a0 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -10,6 +10,7 @@ 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; @@ -35,44 +36,22 @@ import frc4388.utility.status.FaultReporter; import frc4388.utility.status.Queryable; public class Vision extends SubsystemBase implements Queryable { - - // private PhotonCamera leftCamera; - // private PhotonCamera rightCamera; - private PhotonCamera[] cameras; private PhotonPoseEstimator[] estimators; - private List poses = new ArrayList<>(); - private boolean isTagDetected = false; - private boolean isTagProcessed = false; - - private double lastLatency = 0; - - public double getLastLatency() { - return lastLatency; + @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(); } - public Pose2d lastVisionPose = new Pose2d(); - private Pose2d lastPhysOdomPose = new Pose2d(); - - private Matrix curStdDevs; + private VisionState state = new VisionState(); private Field2d field = new Field2d(); - - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbTagDetected = subsystemLayout - .add("Tag Detected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - - GenericEntry sbTagProcessed = subsystemLayout - .add("Tag Processed", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){ FaultReporter.register(this); SmartDashboard.putData(field); @@ -98,17 +77,17 @@ public class Vision extends SubsystemBase implements Queryable { private void update() { - isTagProcessed = false; - isTagDetected = false; + state.isTagProcessed = false; + state.isTagDetected = false; // Instant now = Instant.now(); // int cams = 0; - // double latency = 0; + double latency = 0; // Pose2d pose = null; - poses.clear(); + state.poses.clear(); for(int i = 0; i < cameras.length; i++){ PhotonCamera camera = cameras[i]; @@ -122,9 +101,9 @@ public class Vision extends SubsystemBase implements Queryable { var result = results.get(results.size()-1); - // latency += result.getTimestampSeconds(); + latency += result.getTimestampSeconds(); - isTagDetected = isTagDetected | result.hasTargets(); + state.isTagDetected = state.isTagDetected | result.hasTargets(); // If there are no tags if(!result.hasTargets()) @@ -136,7 +115,7 @@ public class Vision extends SubsystemBase implements Queryable { if(estimatedRobotPose.isEmpty()) continue; - poses.add(estimatedRobotPose.get()); + state.poses.add(estimatedRobotPose.get()); // if(pose == null) // pose = estimatedRobotPose.get().estimatedPose.toPose2d(); @@ -150,7 +129,7 @@ public class Vision extends SubsystemBase implements Queryable { // Yaw += (pose.getRotation().getDegrees() + 180) % 360; // cams++; - isTagProcessed = true; + state.isTagProcessed = true; } @@ -244,22 +223,13 @@ public class Vision extends SubsystemBase implements Queryable { // } // } - /** - * Returns the latest standard deviations of the estimated pose from {@link - * #getEstimatedGlobalPose()}, for use with {@link - * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should - * only be used when there are targets visible. - */ - public Matrix getEstimationStdDevs() { - return curStdDevs; - } public void setLastOdomPose(Optional pose){ if(pose.isPresent()) - lastPhysOdomPose = pose.get(); + state.lastPhysOdomPose = pose.get(); } // public double getLastOdomSpeed(){ @@ -267,8 +237,8 @@ public class Vision extends SubsystemBase implements Queryable { // } public Pose2d getPose2d() { - if(lastPhysOdomPose != null) - return lastPhysOdomPose; + if(state.lastPhysOdomPose != null) + return state.lastPhysOdomPose; // if(lastVisionPose != null) // return lastVisionPose; @@ -281,12 +251,12 @@ public class Vision extends SubsystemBase implements Queryable { } public boolean isTag(){ - return isTagDetected && isTagProcessed; + return state.isTagDetected && state.isTagProcessed; } public void addVisionMeasurement( SwerveDrivetrain drivetrain){ - for(EstimatedRobotPose pose : poses){ + for(EstimatedRobotPose pose : state.poses){ drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds)); } } @@ -303,19 +273,6 @@ public class Vision extends SubsystemBase implements Queryable { return "Vision"; } -// GenericEntry sbShiftState = subsystemLayout -// .add("Shift State", 0) -// .withWidget(BuiltInWidgets.kNumberBar) -// .getEntry(); - - - // @Override - // public void queryStatus() { - // sbTagDetected.setBoolean(isTagDetected); - // sbTagProcessed.setBoolean(isTagProcessed); - // // field.setRobotPose(getPose2d()); - // } - @Override public Status diagnosticStatus() { return new Status(); diff --git a/src/main/java/frc4388/utility/status/Alerts.java b/src/main/java/frc4388/utility/status/Alerts.java new file mode 100644 index 0000000..c8e9c1c --- /dev/null +++ b/src/main/java/frc4388/utility/status/Alerts.java @@ -0,0 +1,14 @@ +package frc4388.utility.status; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +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); + } +}