From: iefomit Date: Sun, 22 Feb 2026 22:12:48 +0000 (-0800) Subject: unused imports, code cleanup X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=34f8f9c50df92ed062c66ad0c0c7cb6dcecd4062;p=FRC2026.git unused imports, code cleanup --- diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java index 28775d4..7f8a613 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -3,22 +3,17 @@ package frc.robot.commands.drive_comm; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Rectangle2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Robot; import frc.robot.constants.swerve.DriveConstants; import frc.robot.controls.BaseDriverConfig; -import frc.robot.controls.PS5ControllerDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.util.TrenchAssist.TrenchAssist; import frc.robot.util.TrenchAssist.TrenchAssist2; import frc.robot.util.TrenchAssist.TrenchAssistConstants; import frc.robot.util.Vision.DriverAssist; -import lib.controllers.PS5Controller.PS5Axis; /** * Default drive command. Drives robot using driver controls. diff --git a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java index fc908dc..f3c371c 100644 --- a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java +++ b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.constants.VisionConstants; import frc.robot.controls.BaseDriverConfig; -import frc.robot.controls.PS5ControllerDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.util.Vision.DetectedObject; diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index d943631..f512cd6 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -38,14 +38,13 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { private LinearClimb climb; public PS5ControllerDriverConfig( - Drivetrain drive, - Shooter shooter, - Turret turret, - Hood hood, - Intake intake, - Spindexer spindexer, - LinearClimb climb - ){ + Drivetrain drive, + Shooter shooter, + Turret turret, + Hood hood, + Intake intake, + Spindexer spindexer, + LinearClimb climb) { super(drive); this.shooter = shooter; this.turret = turret; @@ -76,11 +75,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { // TrenchAlign - on TRIANGLE button driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true))) - .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAlign(false))); + .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAlign(false))); // TrenchAssist - on RB button driver.get(PS5Button.RB).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true))) - .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAssist(false))); + .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAssist(false))); // Intake if (intake != null) { @@ -97,24 +96,24 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { })); // Retract if hold for 3 seconds - driver.get(PS5Button.CROSS).debounce(3.0).onTrue(new InstantCommand(()->{ + driver.get(PS5Button.CROSS).debounce(3.0).onTrue(new InstantCommand(() -> { intake.retract(); intakeBoolean = true; })); // Calibration - driver.get(PS5Button.OPTIONS).onTrue(new InstantCommand(()->{ + driver.get(PS5Button.OPTIONS).onTrue(new InstantCommand(() -> { intake.calibrate(); - })).onFalse(new InstantCommand(()->{ + })).onFalse(new InstantCommand(() -> { intake.stopCalibrating(); })); } // Spindexer - if (spindexer != null){ + if (spindexer != null) { // Will only run if we are not calling default shoot command - driver.get(PS5Button.LB).onTrue(new InstantCommand(()-> spindexer.maxSpindexer())) - .onFalse(new InstantCommand(()-> spindexer.stopSpindexer())); + driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> spindexer.maxSpindexer())) + .onFalse(new InstantCommand(() -> spindexer.stopSpindexer())); } // Auto shoot diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 4fe7b9c..21afaf3 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -55,29 +55,28 @@ public class Drivetrain extends SubsystemBase { public static Lock odometryLock = new ReentrantLock(); - private SwerveSetpoint currentSetpoint = - new SwerveSetpoint( - new ChassisSpeeds(), - new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }); + private SwerveSetpoint currentSetpoint = new SwerveSetpoint( + new ChassisSpeeds(), + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }); // Odometry private final SwerveDrivePoseEstimator poseEstimator; // Vision private final Vision vision; - // PID Controllers for chassis movement private final PIDController xController; private final PIDController yController; private final PIDController rotationController; // If vision is enabled for drivetrain odometry updating - // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED instead + // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED + // instead private boolean visionEnabled = true; // Disables vision for the first few seconds after deploying @@ -95,7 +94,7 @@ public class Drivetrain extends SubsystemBase { private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(); // The pose supplier to drive to - private Supplier desiredPoSupplier = ()->null; + private Supplier desiredPoSupplier = () -> null; private SwerveModulePose modulePoses; @@ -112,7 +111,6 @@ public class Drivetrain extends SubsystemBase { private Rotation2d rawGyroRotation = new Rotation2d(); - /** * Creates a new Swerve Style Drivetrain. */ @@ -123,11 +121,11 @@ public class Drivetrain extends SubsystemBase { this.gyroIO = gyroIO; ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4); - if(RobotBase.isReal()){ + if (RobotBase.isReal()) { Arrays.stream(constants).forEach(moduleConstants -> { modules[moduleConstants.ordinal()] = new Module(moduleConstants); }); - }else{ + } else { Arrays.stream(constants).forEach(moduleConstants -> { modules[moduleConstants.ordinal()] = new ModuleSim(moduleConstants); }); @@ -148,10 +146,9 @@ public class Drivetrain extends SubsystemBase { new Pose2d(), // Defaults, except trust pigeon more VecBuilder.fill(0.1, 0.1, 0), - VisionConstants.VISION_STD_DEVS - ); - poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS); - + VisionConstants.VISION_STD_DEVS); + poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS); + // initialize PID controllers xController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D); yController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D); @@ -162,19 +159,18 @@ public class Drivetrain extends SubsystemBase { PhoenixOdometryThread.getInstance().start(); modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS); - PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); - //PPLibTelemetry.enableCompetitionMode(); + // PPLibTelemetry.enableCompetitionMode(); } public void close() { @@ -193,12 +189,11 @@ public class Drivetrain extends SubsystemBase { module.periodic(); } odometryLock.unlock(); - // Update odometry - double[] sampleTimestamps = - gyroInputs.odometryYawTimestamps; // All signals are sampled together + // Update odometry + double[] sampleTimestamps = gyroInputs.odometryYawTimestamps; // All signals are sampled together int sampleCount = sampleTimestamps.length; SwerveModulePosition[][] positions = new SwerveModulePosition[4][]; - for(int i = 0; i < modules.length; i++){ + for (int i = 0; i < modules.length; i++) { positions[i] = modules[i].getOdometryPositions(); sampleCount = Math.min(sampleCount, positions[i].length); } @@ -207,7 +202,7 @@ public class Drivetrain extends SubsystemBase { SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { modulePositions[moduleIndex] = positions[moduleIndex][i]; - } + } // Use the real gyro angle rawGyroRotation = gyroInputs.odometryYawPositions[i]; // Apply update @@ -224,13 +219,14 @@ public class Drivetrain extends SubsystemBase { * @param xSpeed speed of the robot in the x direction (forward) in m/s * @param ySpeed speed of the robot in the y direction (sideways) in m/s * @param rot angular rate of the robot in rad/s - * @param fieldRelative whether the provided x and y speeds are relative to the field + * @param fieldRelative whether the provided x and y speeds are relative to the + * field * @param isOpenLoop whether to use velocity control for the drive motors */ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) { // rot = headingControl(rot, xSpeed, ySpeed); ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME); - if(fieldRelative){ + if (fieldRelative) { speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw()); } setChassisSpeeds(speeds, isOpenLoop); @@ -242,19 +238,22 @@ public class Drivetrain extends SubsystemBase { * @param xSpeed speed of the robot in the x direction (forward) * @param ySpeed speed of the robot in the y direction (sideways) * @param heading target heading of the robot in radians - * @param fieldRelative whether the provided x and y speeds are relative to the field + * @param fieldRelative whether the provided x and y speeds are relative to the + * field */ public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) { double rot = rotationController.calculate(getYaw().getRadians(), heading); ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot); - if(fieldRelative){ + if (fieldRelative) { speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw()); } setChassisSpeeds(speeds, false); } /** - * Runs the PID controllers with the provided x, y, and rot values. Then, calls {@link #drive(double, double, double, boolean, boolean)} using the PID outputs. + * Runs the PID controllers with the provided x, y, and rot values. Then, calls + * {@link #drive(double, double, double, boolean, boolean)} using the PID + * outputs. * This is based on the odometry of the chassis. * * @param x the position to move to in the x, in meters @@ -279,23 +278,24 @@ public class Drivetrain extends SubsystemBase { // Update the swerve module poses modulePoses.update(); - if(modulePoses.slipped()){ + if (modulePoses.slipped()) { slipped = true; } Pose2d pose2 = getPose(); // Even if vision is disabled, it should still update inputs - // This prevents it from storing a lot of unread results, and it could be useful for replays - if(vision != null){ + // This prevents it from storing a lot of unread results, and it could be useful + // for replays + if (vision != null) { vision.updateInputs(); } - if(VisionConstants.ENABLED){ - if(vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)){ - vision.updateOdometry(poseEstimator, time->getPoseAt(time).getRotation().getRadians(), slipped); + if (VisionConstants.ENABLED) { + if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) { + vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped); - if(vision.canSeeTag()){ + if (vision.canSeeTag()) { slipped = false; modulePoses.reset(); } @@ -303,32 +303,38 @@ public class Drivetrain extends SubsystemBase { } Pose2d pose3 = getPose(); - + // Reset the pose to a position on the field if it is too far off the field - // This uses nearField() instead of onField() so we don't reset the odometry when the wheels slip near the edge of the field + // This uses nearField() instead of onField() so we don't reset the odometry + // when the wheels slip near the edge of the field // This is meant for poses that are caused by errors - if(!Vision.nearField(prevPose)){ - // If the pose at the beginning of the method is off the field, reset to a position in the middle of the field + if (!Vision.nearField(prevPose)) { + // If the pose at the beginning of the method is off the field, reset to a + // position in the middle of the field // Use the rotation of the pose after updating odometry so the yaw is right - prevPose = new Pose2d(FieldConstants.field.getFieldLength()/2, FieldConstants.field.getFieldWidth()/2, pose2.getRotation()); + prevPose = new Pose2d(FieldConstants.field.getFieldLength() / 2, FieldConstants.field.getFieldWidth() / 2, + pose2.getRotation()); resetOdometry(prevPose); - }else if(!Vision.nearField(pose2)){ - // if the drivetrain pose is off the field, reset our odometry to the pose before(this is the right pose) + } else if (!Vision.nearField(pose2)) { + // if the drivetrain pose is off the field, reset our odometry to the pose + // before(this is the right pose) // Keep the rotation from pose2 so yaw is correct for driver prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation()); resetOdometry(prevPose); - }else if(!Vision.nearField(pose3)){ - //if our vision+drivetrain odometry isn't near the field, reset our odometry to the pose before(this is the right pose) + } else if (!Vision.nearField(pose3)) { + // if our vision+drivetrain odometry isn't near the field, reset our odometry to + // the pose before(this is the right pose) resetOdometry(pose2); prevPose = pose2; - }else{ + } else { // Set the previous pose to the current pose if we need to return to that prevPose = pose3; } // if (Robot.isSimulation()) { - // pigeon.getSimState().addYaw( - // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond * Constants.LOOP_TIME)); + // pigeon.getSimState().addYaw( + // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond + // * Constants.LOOP_TIME)); // } } @@ -339,7 +345,6 @@ public class Drivetrain extends SubsystemBase { Arrays.stream(modules).forEach(Module::stop); } - // GETTERS AND SETTERS private boolean trenchAssist = false; @@ -353,18 +358,19 @@ public class Drivetrain extends SubsystemBase { return trenchAlign; } - public void setTrenchAssist(boolean target){ + public void setTrenchAssist(boolean target) { trenchAssist = target; } - public void setTrenchAlign(boolean target){ + public void setTrenchAlign(boolean target) { trenchAlign = target; } /** * Sets the desired states for all swerve modules. * - * @param swerveModuleStates an array of module states to set swerve modules to. Order of the array matters here! + * @param swerveModuleStates an array of module states to set swerve modules to. + * Order of the array matters here! */ public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) { // makes sure speeds of modules don't exceed maximum allowed @@ -379,37 +385,38 @@ public class Drivetrain extends SubsystemBase { * Sets the chassis speeds of the robot. * * @param chassisSpeeds the target chassis speeds - * @param isOpenLoop if open loop control should be used for the drive velocity + * @param isOpenLoop if open loop control should be used for the drive + * velocity */ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) { - if(DriveConstants.USE_ACTUAL_SPEED){ + if (DriveConstants.USE_ACTUAL_SPEED) { SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates()); currentSetpoint = setpointGenerator.generateSetpoint( - DriveConstants.MODULE_LIMITS, - centerOfMassHeight, - currentState, chassisSpeeds, - Constants.LOOP_TIME); - }else{ + DriveConstants.MODULE_LIMITS, + centerOfMassHeight, + currentState, chassisSpeeds, + Constants.LOOP_TIME); + } else { currentSetpoint = setpointGenerator.generateSetpoint( - DriveConstants.MODULE_LIMITS, - centerOfMassHeight, - currentSetpoint, chassisSpeeds, - Constants.LOOP_TIME); + DriveConstants.MODULE_LIMITS, + centerOfMassHeight, + currentSetpoint, chassisSpeeds, + Constants.LOOP_TIME); } SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates(); setModuleStates(swerveModuleStates, isOpenLoop); } - public void setDriveVoltages(Voltage voltage){ - for (int i = 0; i module.setStateDeadband(stateDeadBand)); } + public void setOptimized(boolean optimized) { Arrays.stream(modules).forEach(module -> module.setOptimize(optimized)); } - public void setVisionEnabled(boolean enabled){ + public void setVisionEnabled(boolean enabled) { visionEnabled = enabled; } - public void setIsAlign(boolean isAlign){ + public void setIsAlign(boolean isAlign) { this.isAlign = isAlign; } - public boolean getIsAlign(){ + + public boolean getIsAlign() { return isAlign; } /** * Calculates chassis speed of drivetrain using the current SwerveModuleStates + * * @return ChassisSpeeds object - * This is often used as an input for other methods + * This is often used as an input for other methods */ public ChassisSpeeds getChassisSpeeds() { return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates()); @@ -490,13 +503,14 @@ public class Drivetrain extends SubsystemBase { /** * Gets the state of each module + * * @return An array of 4 SwerveModuleStates */ - public SwerveModuleState[] getModuleStates(){ + public SwerveModuleState[] getModuleStates() { return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); } - public SwerveSetpoint getCurrSetpoint(){ + public SwerveSetpoint getCurrSetpoint() { return currentSetpoint; } @@ -510,7 +524,7 @@ public class Drivetrain extends SubsystemBase { /** * @return an array of modules */ - public Module[] getModules(){ + public Module[] getModules() { return modules; } @@ -545,30 +559,33 @@ public class Drivetrain extends SubsystemBase { /** * Sets the angle to align to + * * @param newAngle The new angle in radians, can be set to null */ - public void setAlignAngle(Double newAngle){ + public void setAlignAngle(Double newAngle) { alignAngle = newAngle; } /** * Returns whether or not the robot is at the input align angle + * * @return true if it within tolerance the align angle, false otherwise */ - public boolean atAlignAngle(){ - if(alignAngle == null){ + public boolean atAlignAngle() { + if (alignAngle == null) { return false; } double diff = Math.abs(alignAngle - getYaw().getRadians()); - return diff < DriveConstants.HEADING_TOLERANCE || diff > 2*Math.PI - DriveConstants.HEADING_TOLERANCE; + return diff < DriveConstants.HEADING_TOLERANCE || diff > 2 * Math.PI - DriveConstants.HEADING_TOLERANCE; } /** * Gets the angle to align to + * * @return The angle in radians */ - public double getAlignAngle(){ - if(alignAngle != null){ + public double getAlignAngle() { + if (alignAngle != null) { return alignAngle; } return 0; @@ -576,54 +593,62 @@ public class Drivetrain extends SubsystemBase { /** * Sets vision to only use certain April tags + * * @param ids An array of the tags to only use */ - public void onlyUseTags(int[] ids){ - if(vision != null){ + public void onlyUseTags(int[] ids) { + if (vision != null) { vision.onlyUse(ids); } } + /** * Returns if vision has seen an April tag in the last frame + * * @return true if vision saw a tag last frame or if vision is disabled */ - public boolean canSeeTag(){ + public boolean canSeeTag() { // if no vision system, then return true - if (vision == null) return true; + if (vision == null) + return true; return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED; } /** * Gets the pose at a previous time + * * @param timestamp The timestamp of the pose to get - * @return The pose, null if there are no poses yet, or the current pose if timestamp < 0 + * @return The pose, null if there are no poses yet, or the current pose if + * timestamp < 0 */ - public Pose2d getPoseAt(double timestamp){ - if(timestamp < 0){ + public Pose2d getPoseAt(double timestamp) { + if (timestamp < 0) { return getPose(); } Optional pose = poseEstimator.sampleAt(timestamp); - if(pose.isPresent()){ + if (pose.isPresent()) { return pose.get(); - }else{ + } else { return null; } } /** * Uses pigeon and rotational input to return a rotation that accounts for drift + * * @return A rotation */ - public double headingControl(double rot, double xSpeed, double ySpeed){ - if((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004)&&EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed),0,0.1))||!EqualsUtil.epsilonEquals(rot, 0, 0.0004)){ - drive_turning = true; - currentHeading = getYaw().getRadians(); - } - else{ + public double headingControl(double rot, double xSpeed, double ySpeed) { + if ((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004) + && EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed), 0, 0.1)) + || !EqualsUtil.epsilonEquals(rot, 0, 0.0004)) { + drive_turning = true; + currentHeading = getYaw().getRadians(); + } else { drive_turning = false; } - if (!drive_turning){ + if (!drive_turning) { rotationController.setSetpoint(currentHeading); double output = rotationController.calculate(getYaw().getRadians()); rot = Math.abs(output) > Math.abs(rot) ? output : rot; @@ -642,9 +667,11 @@ public class Drivetrain extends SubsystemBase { public PIDController getXController() { return xController; } + public PIDController getYController() { return yController; } + public PIDController getRotationController() { return rotationController; } @@ -652,35 +679,39 @@ public class Drivetrain extends SubsystemBase { /** * Set the desired pose to drive to * This will enable driver assist to go to the pose - * @param supplier The supplier for the desired pose, use ()->null to not use a desired pose + * + * @param supplier The supplier for the desired pose, use ()->null to not use a + * desired pose */ - public void setDesiredPose(Supplier supplier){ + public void setDesiredPose(Supplier supplier) { desiredPoSupplier = supplier; } /** * Set the desired pose to drive to * This will enable driver assist to go to the pose + * * @param pose The Pose2d to drive to */ - public void setDesiredPose(Pose2d pose){ - setDesiredPose(()->pose); + public void setDesiredPose(Pose2d pose) { + setDesiredPose(() -> pose); } /** * Gets the current desired pose, or null if there is no desired pose + * * @return The Pose2d if it exists, null otherwise */ - public Pose2d getDesiredPose(){ + public Pose2d getDesiredPose() { return desiredPoSupplier.get(); } - public boolean atSetpoint(){ + public boolean atSetpoint() { Pose2d pose = getDesiredPose(); return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025; } - public SwerveModulePose getSwerveModulePose(){ + public SwerveModulePose getSwerveModulePose() { return modulePoses; } @@ -691,7 +722,7 @@ public class Drivetrain extends SubsystemBase { double angularVelocity = getAngularRate(3); double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME; previousAngularVelocity = angularVelocity; - + double pigeonOffsetX = 0.082677; double pigeonOffsetY = 0.030603444; @@ -700,20 +731,20 @@ public class Drivetrain extends SubsystemBase { return Math.hypot(totalX, totalY); } - + @AutoLogOutput(key = "Drivetrain/AccelerationFaults") public boolean accelerationOverMax() { return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL; } - public void setCenterOfMass(double height){ + public void setCenterOfMass(double height) { centerOfMassHeight = height; } - public void alignWheels(){ + public void alignWheels() { SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0)); - setModuleStates(new SwerveModuleState[]{ - state, state, state, state + setModuleStates(new SwerveModuleState[] { + state, state, state, state }, false); } } diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 05bf846..a707680 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; -import frc.robot.subsystems.spindexer.SpindexerIO; public class Spindexer extends SubsystemBase implements SpindexerIO { private TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID, Constants.CANIVORE_SUB); diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java index e35d0b9..ab26d8c 100644 --- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java @@ -1,11 +1,8 @@ package frc.robot.util.TrenchAssist; -import static edu.wpi.first.units.Units.Rotation; - import java.util.Optional; import org.littletonrobotics.junction.Logger; -import org.opencv.core.Point; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rectangle2d; @@ -14,17 +11,8 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Robot; import frc.robot.constants.swerve.DriveConstants; -import frc.robot.controls.BaseDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.util.Vision.DriverAssist; -import frc.robot.util.TrenchAssist.TrenchAssistConstants; public class TrenchAssist { diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java index 0a2f142..6782d98 100644 --- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java @@ -1,41 +1,22 @@ package frc.robot.util.TrenchAssist; -import static edu.wpi.first.units.Units.Rotation; - -import java.util.Optional; - -import org.littletonrobotics.junction.Logger; -import org.opencv.core.Point; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Robot; -import frc.robot.constants.swerve.DriveConstants; -import frc.robot.controls.BaseDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.util.Vision.DriverAssist; -import frc.robot.util.TrenchAssist.TrenchAssistConstants; public class TrenchAssist2 { private static final double SCALE = 1.0; public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds) { - //ChassisSpeeds speedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, drive.getYaw().unaryMinus()); //this singular unary minus is best we can get + // ChassisSpeeds speedsFieldRelative = + // ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, + // drive.getYaw().unaryMinus()); //this singular unary minus is best we can get double distanceFromSlideLatitude; - if (drive.getPose().getY() > (8.07 / 2.0)){ + if (drive.getPose().getY() > (8.07 / 2.0)) { distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[0]); } else { distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[1]); @@ -43,28 +24,31 @@ public class TrenchAssist2 { var impulse = -distanceFromSlideLatitude * SCALE; - ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, impulse, chassisSpeeds.omegaRadiansPerSecond); + ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, impulse, + chassisSpeeds.omegaRadiansPerSecond); - var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond); + var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond); var y = new Translation2d(x.vxMetersPerSecond, x.vyMetersPerSecond).rotateBy(drive.getYaw()); - var z = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond), drive.getYaw()); + var z = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond), + drive.getYaw()); return z; } - public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive){ + public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive) { Rotation2d yaw = drive.getYaw(); Translation2d robotRelative = translation.rotateBy(yaw); return new ChassisSpeeds(robotRelative.getX(), robotRelative.getY(), 0.0); } - public static Translation2d convertToTranslation2dFieldRelative(ChassisSpeeds speeds, Drivetrain drive){ + public static Translation2d convertToTranslation2dFieldRelative(ChassisSpeeds speeds, Drivetrain drive) { Rotation2d yaw = drive.getYaw(); Translation2d robotTranslation = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - return robotTranslation.rotateBy(yaw.times(-1)); + return robotTranslation.rotateBy(yaw.times(-1)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java index 1c9c5ca..8bb4ef4 100644 --- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java @@ -2,29 +2,29 @@ package frc.robot.util.TrenchAssist; import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; public class TrenchAssistConstants { - public static final Rectangle2d[] OBSTACLES = new Rectangle2d[]{ - new Rectangle2d(new Translation2d(4.03, 1.28), new Translation2d(5.22, 1.58)), - new Rectangle2d(new Translation2d(4.03, 8.07 - 1.28), new Translation2d(5.22, 8.07 - 1.58)), - new Rectangle2d(new Translation2d(11.32, 1.28), new Translation2d(12.51, 1.58)), - new Rectangle2d(new Translation2d(11.32, 8.07 - 1.28), new Translation2d(12.51, 8.07 - 1.58)), - }; //8.07m + public static final Rectangle2d[] OBSTACLES = new Rectangle2d[] { + new Rectangle2d(new Translation2d(4.03, 1.28), new Translation2d(5.22, 1.58)), + new Rectangle2d(new Translation2d(4.03, 8.07 - 1.28), new Translation2d(5.22, 8.07 - 1.58)), + new Rectangle2d(new Translation2d(11.32, 1.28), new Translation2d(12.51, 1.58)), + new Rectangle2d(new Translation2d(11.32, 8.07 - 1.28), new Translation2d(12.51, 8.07 - 1.58)), + }; // 8.07m - public static final Rectangle2d[] ALIGN_ZONES = new Rectangle2d[]{ - new Rectangle2d(new Translation2d(4.03 - .5, 0), new Translation2d(5.22 + .5, 3)), - new Rectangle2d(new Translation2d(4.03 - .5, 8.07), new Translation2d(5.22 + .5, 8.07 - 3)), - new Rectangle2d(new Translation2d(11.32 - .5, 0), new Translation2d(12.51 + .5, 3)), - new Rectangle2d(new Translation2d(11.32 - .5, 8.07), new Translation2d(12.51 + .5, 8.07 - 3)), - new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)), + public static final Rectangle2d[] ALIGN_ZONES = new Rectangle2d[] { + new Rectangle2d(new Translation2d(4.03 - .5, 0), new Translation2d(5.22 + .5, 3)), + new Rectangle2d(new Translation2d(4.03 - .5, 8.07), new Translation2d(5.22 + .5, 8.07 - 3)), + new Rectangle2d(new Translation2d(11.32 - .5, 0), new Translation2d(12.51 + .5, 3)), + new Rectangle2d(new Translation2d(11.32 - .5, 8.07), new Translation2d(12.51 + .5, 8.07 - 3)), + new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)), }; - public static final double[] SLIDE_LATITUDES = new double[]{ - // 8.07 - Units.inchesToMeters(30.0), - // Units.inchesToMeters(30.0), should be accurate, i think our field is slightly too small - 6.550, - 0.668, + public static final double[] SLIDE_LATITUDES = new double[] { + // 8.07 - Units.inchesToMeters(30.0), + // Units.inchesToMeters(30.0), should be accurate, i think our field is slightly + // too small + 6.550, + 0.668, };