diff --git a/.gitignore b/.gitignore index 4dcb339..0384fc9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,24 @@ # build file .gradle build + +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* +replay_pid* diff --git a/bin/main/frc/FSLib2025/math/LinearRegression.class b/bin/main/frc/FSLib2025/math/LinearRegression.class deleted file mode 100644 index e689ea7..0000000 Binary files a/bin/main/frc/FSLib2025/math/LinearRegression.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/math/Maths.class b/bin/main/frc/FSLib2025/math/Maths.class deleted file mode 100644 index 3b73afe..0000000 Binary files a/bin/main/frc/FSLib2025/math/Maths.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/state_machine/StateMachine.class b/bin/main/frc/FSLib2025/state_machine/StateMachine.class deleted file mode 100644 index 84c3364..0000000 Binary files a/bin/main/frc/FSLib2025/state_machine/StateMachine.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/state_machine/SuperstructureState.class b/bin/main/frc/FSLib2025/state_machine/SuperstructureState.class deleted file mode 100644 index 8f00c80..0000000 Binary files a/bin/main/frc/FSLib2025/state_machine/SuperstructureState.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/swerve/OnboardModuleState.class b/bin/main/frc/FSLib2025/swerve/OnboardModuleState.class deleted file mode 100644 index 33e6366..0000000 Binary files a/bin/main/frc/FSLib2025/swerve/OnboardModuleState.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/swerve/SwerveModuleConfig.class b/bin/main/frc/FSLib2025/swerve/SwerveModuleConfig.class deleted file mode 100644 index 189de66..0000000 Binary files a/bin/main/frc/FSLib2025/swerve/SwerveModuleConfig.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/swerve/SwerveModuleConstants.class b/bin/main/frc/FSLib2025/swerve/SwerveModuleConstants.class deleted file mode 100644 index 44bea4f..0000000 Binary files a/bin/main/frc/FSLib2025/swerve/SwerveModuleConstants.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/util/LocalADStarAK$ADStarIO.class b/bin/main/frc/FSLib2025/util/LocalADStarAK$ADStarIO.class deleted file mode 100644 index 4754606..0000000 Binary files a/bin/main/frc/FSLib2025/util/LocalADStarAK$ADStarIO.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/util/LocalADStarAK.class b/bin/main/frc/FSLib2025/util/LocalADStarAK.class deleted file mode 100644 index c076f34..0000000 Binary files a/bin/main/frc/FSLib2025/util/LocalADStarAK.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightResults.class b/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightResults.class deleted file mode 100644 index 4cae381..0000000 Binary files a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightResults.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Barcode.class b/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Barcode.class deleted file mode 100644 index 0781c52..0000000 Binary files a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Barcode.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Classifier.class b/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Classifier.class deleted file mode 100644 index 7ccad42..0000000 Binary files a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Classifier.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Detector.class b/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Detector.class deleted file mode 100644 index 12a5110..0000000 Binary files a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Detector.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Fiducial.class b/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Fiducial.class deleted file mode 100644 index eae077e..0000000 Binary files a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Fiducial.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Retro.class b/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Retro.class deleted file mode 100644 index d1ae711..0000000 Binary files a/bin/main/frc/FSLib2025/vision/LimelightHelpers$LimelightTarget_Retro.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/vision/LimelightHelpers$PoseEstimate.class b/bin/main/frc/FSLib2025/vision/LimelightHelpers$PoseEstimate.class deleted file mode 100644 index 5f360f5..0000000 Binary files a/bin/main/frc/FSLib2025/vision/LimelightHelpers$PoseEstimate.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/vision/LimelightHelpers$RawDetection.class b/bin/main/frc/FSLib2025/vision/LimelightHelpers$RawDetection.class deleted file mode 100644 index b0787e6..0000000 Binary files a/bin/main/frc/FSLib2025/vision/LimelightHelpers$RawDetection.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/vision/LimelightHelpers$RawFiducial.class b/bin/main/frc/FSLib2025/vision/LimelightHelpers$RawFiducial.class deleted file mode 100644 index 4e24253..0000000 Binary files a/bin/main/frc/FSLib2025/vision/LimelightHelpers$RawFiducial.class and /dev/null differ diff --git a/bin/main/frc/FSLib2025/vision/LimelightHelpers.class b/bin/main/frc/FSLib2025/vision/LimelightHelpers.class deleted file mode 100644 index f884d45..0000000 Binary files a/bin/main/frc/FSLib2025/vision/LimelightHelpers.class and /dev/null differ diff --git a/bin/main/frc/robot/Constants$RobotConstants.class b/bin/main/frc/robot/Constants$RobotConstants.class deleted file mode 100644 index 434dd6e..0000000 Binary files a/bin/main/frc/robot/Constants$RobotConstants.class and /dev/null differ diff --git a/bin/main/frc/robot/Constants$SuperstructureConstants.class b/bin/main/frc/robot/Constants$SuperstructureConstants.class deleted file mode 100644 index 901da05..0000000 Binary files a/bin/main/frc/robot/Constants$SuperstructureConstants.class and /dev/null differ diff --git a/bin/main/frc/robot/Constants$SwerveConstants.class b/bin/main/frc/robot/Constants$SwerveConstants.class deleted file mode 100644 index 248fca2..0000000 Binary files a/bin/main/frc/robot/Constants$SwerveConstants.class and /dev/null differ diff --git a/bin/main/frc/robot/Constants.class b/bin/main/frc/robot/Constants.class deleted file mode 100644 index db02e98..0000000 Binary files a/bin/main/frc/robot/Constants.class and /dev/null differ diff --git a/bin/main/frc/robot/Main.class b/bin/main/frc/robot/Main.class deleted file mode 100644 index fb85994..0000000 Binary files a/bin/main/frc/robot/Main.class and /dev/null differ diff --git a/bin/main/frc/robot/Robot.class b/bin/main/frc/robot/Robot.class deleted file mode 100644 index d4b7170..0000000 Binary files a/bin/main/frc/robot/Robot.class and /dev/null differ diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class deleted file mode 100644 index 402aec1..0000000 Binary files a/bin/main/frc/robot/RobotContainer.class and /dev/null differ diff --git a/bin/main/frc/robot/commands/TeleopSwerve.class b/bin/main/frc/robot/commands/TeleopSwerve.class deleted file mode 100644 index af250cb..0000000 Binary files a/bin/main/frc/robot/commands/TeleopSwerve.class and /dev/null differ diff --git a/bin/main/frc/robot/subsystems/Elevator.class b/bin/main/frc/robot/subsystems/Elevator.class deleted file mode 100644 index c809c89..0000000 Binary files a/bin/main/frc/robot/subsystems/Elevator.class and /dev/null differ diff --git a/bin/main/frc/robot/subsystems/Grabber.class b/bin/main/frc/robot/subsystems/Grabber.class deleted file mode 100644 index 75ac9b5..0000000 Binary files a/bin/main/frc/robot/subsystems/Grabber.class and /dev/null differ diff --git a/bin/main/frc/robot/subsystems/Intake.class b/bin/main/frc/robot/subsystems/Intake.class deleted file mode 100644 index 41d732b..0000000 Binary files a/bin/main/frc/robot/subsystems/Intake.class and /dev/null differ diff --git a/bin/main/frc/robot/subsystems/LED.class b/bin/main/frc/robot/subsystems/LED.class deleted file mode 100644 index 9c44260..0000000 Binary files a/bin/main/frc/robot/subsystems/LED.class and /dev/null differ diff --git a/bin/main/frc/robot/subsystems/Swerve.class b/bin/main/frc/robot/subsystems/Swerve.class deleted file mode 100644 index dcf5668..0000000 Binary files a/bin/main/frc/robot/subsystems/Swerve.class and /dev/null differ diff --git a/bin/main/frc/robot/subsystems/SwerveModule.class b/bin/main/frc/robot/subsystems/SwerveModule.class deleted file mode 100644 index 979055e..0000000 Binary files a/bin/main/frc/robot/subsystems/SwerveModule.class and /dev/null differ diff --git a/src/main/java/frc/FSLib2025/math/LinearRegression.java b/src/main/java/frc/FSLib2025/math/LinearRegression.java index b10c7e3..58e5f4b 100644 --- a/src/main/java/frc/FSLib2025/math/LinearRegression.java +++ b/src/main/java/frc/FSLib2025/math/LinearRegression.java @@ -1,22 +1,23 @@ package frc.FSLib2025.math; public class LinearRegression { - + private double[][] data; - public LinearRegression (double[][] mapValues) { + public LinearRegression(double[][] mapValues) { data = mapValues; } - public double calculate (double xValues) { + public double calculate(double xValues) { int index = 0; for (double[] i : data) { - if(i[0] >= xValues) break; + if (i[0] >= xValues) + break; index++; } - double dx = xValues - data[index-1][0]; - double x = data[index][0] - data[index-1][0]; - return data[index-1][1] * (1-dx/x) + data[index][1] * dx/x; + double dx = xValues - data[index - 1][0]; + double x = data[index][0] - data[index - 1][0]; + return data[index - 1][1] * (1 - dx / x) + data[index][1] * dx / x; } } diff --git a/src/main/java/frc/FSLib2025/math/Maths.java b/src/main/java/frc/FSLib2025/math/Maths.java index ac3205d..a5a6796 100644 --- a/src/main/java/frc/FSLib2025/math/Maths.java +++ b/src/main/java/frc/FSLib2025/math/Maths.java @@ -1,16 +1,16 @@ package frc.FSLib2025.math; public class Maths { - - public static boolean isWithin (double value, double min, double max) { + + public static boolean isWithin(double value, double min, double max) { return Math.max(min, value) == Math.min(value, max); } - - public static double clamp (double value, double min, double max) { + + public static double clamp(double value, double min, double max) { return Math.min(Math.max(value, min), max); } - public static int clamp (int value, int min, int max) { + public static int clamp(int value, int min, int max) { return Math.min(Math.max(value, min), max); } diff --git a/src/main/java/frc/FSLib2025/swerve/OnboardModuleState.java b/src/main/java/frc/FSLib2025/swerve/OnboardModuleState.java index ebcb6dd..093a960 100644 --- a/src/main/java/frc/FSLib2025/swerve/OnboardModuleState.java +++ b/src/main/java/frc/FSLib2025/swerve/OnboardModuleState.java @@ -5,56 +5,56 @@ public class OnboardModuleState { - /** - * Minimize the change in heading the desired swerve module state would require - * by potentially - * reversing the direction the wheel spins. Customized from WPILib's version to - * include placing in - * appropriate scope for CTRE and REV onboard control as both controllers as of - * writing don't have - * support for continuous input. - * - * @param desiredState The desired state. - * @param currentAngle The current module angle. - */ - public static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) { - double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees()); - double targetSpeed = desiredState.speedMetersPerSecond; - double delta = targetAngle - currentAngle.getDegrees(); - if (Math.abs(delta) > 90) { - targetSpeed = -targetSpeed; - targetAngle = delta > 90 ? (targetAngle -= 180) : (targetAngle += 180); + /** + * Minimize the change in heading the desired swerve module state would require + * by potentially + * reversing the direction the wheel spins. Customized from WPILib's version to + * include placing in + * appropriate scope for CTRE and REV onboard control as both controllers as of + * writing don't have + * support for continuous input. + * + * @param desiredState The desired state. + * @param currentAngle The current module angle. + */ + public static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) { + double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees()); + double targetSpeed = desiredState.speedMetersPerSecond; + double delta = targetAngle - currentAngle.getDegrees(); + if (Math.abs(delta) > 90) { + targetSpeed = -targetSpeed; + targetAngle = delta > 90 ? (targetAngle -= 180) : (targetAngle += 180); + } + return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle)); } - return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle)); - } - /** - * @param scopeReference Current Angle - * @param newAngle Target Angle - * @return Closest angle within scope - */ - private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) { - double lowerBound; - double upperBound; - double lowerOffset = scopeReference % 360; - if (lowerOffset >= 0) { - lowerBound = scopeReference - lowerOffset; - upperBound = scopeReference + (360 - lowerOffset); - } else { - upperBound = scopeReference - lowerOffset; - lowerBound = scopeReference - (360 + lowerOffset); + /** + * @param scopeReference Current Angle + * @param newAngle Target Angle + * @return Closest angle within scope + */ + private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) { + double lowerBound; + double upperBound; + double lowerOffset = scopeReference % 360; + if (lowerOffset >= 0) { + lowerBound = scopeReference - lowerOffset; + upperBound = scopeReference + (360 - lowerOffset); + } else { + upperBound = scopeReference - lowerOffset; + lowerBound = scopeReference - (360 + lowerOffset); + } + while (newAngle < lowerBound) { + newAngle += 360; + } + while (newAngle > upperBound) { + newAngle -= 360; + } + if (newAngle - scopeReference > 180) { + newAngle -= 360; + } else if (newAngle - scopeReference < -180) { + newAngle += 360; + } + return newAngle; } - while (newAngle < lowerBound) { - newAngle += 360; - } - while (newAngle > upperBound) { - newAngle -= 360; - } - if (newAngle - scopeReference > 180) { - newAngle -= 360; - } else if (newAngle - scopeReference < -180) { - newAngle += 360; - } - return newAngle; - } } diff --git a/src/main/java/frc/FSLib2025/swerve/SwerveModuleConfig.java b/src/main/java/frc/FSLib2025/swerve/SwerveModuleConfig.java index 68b48d1..ffb97f6 100644 --- a/src/main/java/frc/FSLib2025/swerve/SwerveModuleConfig.java +++ b/src/main/java/frc/FSLib2025/swerve/SwerveModuleConfig.java @@ -8,8 +8,8 @@ public class SwerveModuleConfig { public int AngleMotorId; public int CancoderId; public Rotation2d AngleOffset; - - public SwerveModuleConfig (int driveMotorId, int angleMotorId, int cancoderId, Rotation2d angleOffset) { + + public SwerveModuleConfig(int driveMotorId, int angleMotorId, int cancoderId, Rotation2d angleOffset) { this.DriveMotorId = driveMotorId; this.AngleMotorId = angleMotorId; this.CancoderId = cancoderId; diff --git a/src/main/java/frc/FSLib2025/util/LocalADStarAK.java b/src/main/java/frc/FSLib2025/util/LocalADStarAK.java index d16c7b3..98984cb 100644 --- a/src/main/java/frc/FSLib2025/util/LocalADStarAK.java +++ b/src/main/java/frc/FSLib2025/util/LocalADStarAK.java @@ -16,134 +16,140 @@ import org.littletonrobotics.junction.inputs.LoggableInputs; public class LocalADStarAK implements Pathfinder { - private final ADStarIO io = new ADStarIO(); - - /** - * Get if a new path has been calculated since the last time a path was retrieved - * - * @return True if a new path is available - */ - @Override - public boolean isNewPathAvailable() { - if (!Logger.hasReplaySource()) { - io.updateIsNewPathAvailable(); - } + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was + * retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } - Logger.processInputs("LocalADStarAK", io); - - return io.isNewPathAvailable; - } - - /** - * Get the most recently calculated path - * - * @param constraints The path constraints to use when creating the path - * @param goalEndState The goal end state to use when creating the path - * @return The PathPlannerPath created from the points calculated by the pathfinder - */ - @Override - public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - if (!Logger.hasReplaySource()) { - io.updateCurrentPathPoints(constraints, goalEndState); + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; } - Logger.processInputs("LocalADStarAK", io); + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the + * pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } - if (io.currentPathPoints.isEmpty()) { - return null; - } + Logger.processInputs("LocalADStarAK", io); - return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); - } - - /** - * Set the start position to pathfind from - * - * @param startPosition Start position on the field. If this is within an obstacle it will be - * moved to the nearest non-obstacle node. - */ - @Override - public void setStartPosition(Translation2d startPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setStartPosition(startPosition); - } - } - - /** - * Set the goal position to pathfind to - * - * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved - * to the nearest non-obstacle node. - */ - @Override - public void setGoalPosition(Translation2d goalPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setGoalPosition(goalPosition); - } - } - - /** - * Set the dynamic obstacles that should be avoided while pathfinding. - * - * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents - * opposite corners of a bounding box. - * @param currentRobotPos The current position of the robot. This is needed to change the start - * position of the path to properly avoid obstacles - */ - @Override - public void setDynamicObstacles( - List> obs, Translation2d currentRobotPos) { - if (!Logger.hasReplaySource()) { - io.adStar.setDynamicObstacles(obs, currentRobotPos); - } - } + if (io.currentPathPoints.isEmpty()) { + return null; + } - private static class ADStarIO implements LoggableInputs { - public LocalADStar adStar = new LocalADStar(); - public boolean isNewPathAvailable = false; - public List currentPathPoints = Collections.emptyList(); + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an + * obstacle it will be + * moved to the nearest non-obstacle node. + */ @Override - public void toLog(LogTable table) { - table.put("IsNewPathAvailable", isNewPathAvailable); - - double[] pointsLogged = new double[currentPathPoints.size() * 2]; - int idx = 0; - for (PathPoint point : currentPathPoints) { - pointsLogged[idx] = point.position.getX(); - pointsLogged[idx + 1] = point.position.getY(); - idx += 2; - } - - table.put("CurrentPathPoints", pointsLogged); + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } } + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle + * it will be moved + * to the nearest non-obstacle node. + */ @Override - public void fromLog(LogTable table) { - isNewPathAvailable = table.get("IsNewPathAvailable", false); - - double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); - - List pathPoints = new ArrayList<>(); - for (int i = 0; i < pointsLogged.length; i += 2) { - pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); - } - - currentPathPoints = pathPoints; + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } } - public void updateIsNewPathAvailable() { - isNewPathAvailable = adStar.isNewPathAvailable(); + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. + * Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to + * change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } } - public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { - PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); - - if (currentPath != null) { - currentPathPoints = currentPath.getAllPathPoints(); - } else { - currentPathPoints = Collections.emptyList(); - } + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } } - } } \ No newline at end of file diff --git a/src/main/java/frc/FSLib2025/vision/LimelightHelpers.java b/src/main/java/frc/FSLib2025/vision/LimelightHelpers.java index e66111f..e8a28fc 100644 --- a/src/main/java/frc/FSLib2025/vision/LimelightHelpers.java +++ b/src/main/java/frc/FSLib2025/vision/LimelightHelpers.java @@ -34,7 +34,6 @@ public class LimelightHelpers { private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - public static class LimelightTarget_Retro { @JsonProperty("t6c_ts") @@ -44,7 +43,7 @@ public static class LimelightTarget_Retro { private double[] robotPose_FieldSpace; @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; + private double[] robotPose_TargetSpace; @JsonProperty("t6t_cs") private double[] targetPose_CameraSpace; @@ -52,45 +51,43 @@ public static class LimelightTarget_Retro { @JsonProperty("t6t_rs") private double[] targetPose_RobotSpace; - public Pose3d getCameraPose_TargetSpace() - { + public Pose3d getCameraPose_TargetSpace() { return toPose3D(cameraPose_TargetSpace); } - public Pose3d getRobotPose_FieldSpace() - { + + public Pose3d getRobotPose_FieldSpace() { return toPose3D(robotPose_FieldSpace); } - public Pose3d getRobotPose_TargetSpace() - { + + public Pose3d getRobotPose_TargetSpace() { return toPose3D(robotPose_TargetSpace); } - public Pose3d getTargetPose_CameraSpace() - { + + public Pose3d getTargetPose_CameraSpace() { return toPose3D(targetPose_CameraSpace); } - public Pose3d getTargetPose_RobotSpace() - { + + public Pose3d getTargetPose_RobotSpace() { return toPose3D(targetPose_RobotSpace); } - public Pose2d getCameraPose_TargetSpace2D() - { + public Pose2d getCameraPose_TargetSpace2D() { return toPose2D(cameraPose_TargetSpace); } - public Pose2d getRobotPose_FieldSpace2D() - { + + public Pose2d getRobotPose_FieldSpace2D() { return toPose2D(robotPose_FieldSpace); } - public Pose2d getRobotPose_TargetSpace2D() - { + + public Pose2d getRobotPose_TargetSpace2D() { return toPose2D(robotPose_TargetSpace); } - public Pose2d getTargetPose_CameraSpace2D() - { + + public Pose2d getTargetPose_CameraSpace2D() { return toPose2D(targetPose_CameraSpace); } - public Pose2d getTargetPose_RobotSpace2D() - { + + public Pose2d getTargetPose_RobotSpace2D() { return toPose2D(targetPose_RobotSpace); } @@ -145,48 +142,46 @@ public static class LimelightTarget_Fiducial { @JsonProperty("t6t_rs") private double[] targetPose_RobotSpace; - public Pose3d getCameraPose_TargetSpace() - { + public Pose3d getCameraPose_TargetSpace() { return toPose3D(cameraPose_TargetSpace); } - public Pose3d getRobotPose_FieldSpace() - { + + public Pose3d getRobotPose_FieldSpace() { return toPose3D(robotPose_FieldSpace); } - public Pose3d getRobotPose_TargetSpace() - { + + public Pose3d getRobotPose_TargetSpace() { return toPose3D(robotPose_TargetSpace); } - public Pose3d getTargetPose_CameraSpace() - { + + public Pose3d getTargetPose_CameraSpace() { return toPose3D(targetPose_CameraSpace); } - public Pose3d getTargetPose_RobotSpace() - { + + public Pose3d getTargetPose_RobotSpace() { return toPose3D(targetPose_RobotSpace); } - public Pose2d getCameraPose_TargetSpace2D() - { + public Pose2d getCameraPose_TargetSpace2D() { return toPose2D(cameraPose_TargetSpace); } - public Pose2d getRobotPose_FieldSpace2D() - { + + public Pose2d getRobotPose_FieldSpace2D() { return toPose2D(robotPose_FieldSpace); } - public Pose2d getRobotPose_TargetSpace2D() - { + + public Pose2d getRobotPose_TargetSpace2D() { return toPose2D(robotPose_TargetSpace); } - public Pose2d getTargetPose_CameraSpace2D() - { + + public Pose2d getTargetPose_CameraSpace2D() { return toPose2D(targetPose_CameraSpace); } - public Pose2d getTargetPose_RobotSpace2D() - { + + public Pose2d getTargetPose_RobotSpace2D() { return toPose2D(targetPose_RobotSpace); } - + @JsonProperty("ta") public double ta; @@ -204,7 +199,7 @@ public Pose2d getTargetPose_RobotSpace2D() @JsonProperty("ts") public double ts; - + public LimelightTarget_Fiducial() { cameraPose_TargetSpace = new double[6]; robotPose_FieldSpace = new double[6]; @@ -244,7 +239,7 @@ public static class LimelightTarget_Classifier { @JsonProperty("typ") public double ty_pixels; - public LimelightTarget_Classifier() { + public LimelightTarget_Classifier() { } } @@ -279,9 +274,9 @@ public LimelightTarget_Detector() { } public static class LimelightResults { - + public String error; - + @JsonProperty("pID") public double pipelineID; @@ -314,13 +309,13 @@ public static class LimelightResults { @JsonProperty("botpose_tagcount") public double botpose_tagcount; - + @JsonProperty("botpose_span") public double botpose_span; - + @JsonProperty("botpose_avgdist") public double botpose_avgdist; - + @JsonProperty("botpose_avgarea") public double botpose_avgarea; @@ -330,11 +325,11 @@ public static class LimelightResults { public Pose3d getBotPose3d() { return toPose3D(botpose); } - + public Pose3d getBotPose3d_wpiRed() { return toPose3D(botpose_wpired); } - + public Pose3d getBotPose3d_wpiBlue() { return toPose3D(botpose_wpiblue); } @@ -342,11 +337,11 @@ public Pose3d getBotPose3d_wpiBlue() { public Pose2d getBotPose2d() { return toPose2D(botpose); } - + public Pose2d getBotPose2d_wpiRed() { return toPose2D(botpose_wpired); } - + public Pose2d getBotPose2d_wpiBlue() { return toPose2D(botpose_wpiblue); } @@ -379,7 +374,6 @@ public LimelightResults() { } - } public static class RawFiducial { @@ -391,8 +385,8 @@ public static class RawFiducial { public double distToRobot = 0; public double ambiguity = 0; - - public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, + double ambiguity) { this.id = id; this.txnc = txnc; this.tync = tync; @@ -417,12 +411,11 @@ public static class RawDetection { public double corner3_X = 0; public double corner3_Y = 0; - - public RawDetection(int classId, double txnc, double tync, double ta, - double corner0_X, double corner0_Y, - double corner1_X, double corner1_Y, - double corner2_X, double corner2_Y, - double corner3_X, double corner3_Y ) { + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y) { this.classId = classId; this.txnc = txnc; this.tync = tync; @@ -446,7 +439,7 @@ public static class PoseEstimate { public double tagSpan; public double avgTagDist; public double avgTagArea; - public RawFiducial[] rawFiducials; + public RawFiducial[] rawFiducials; /** * Makes a PoseEstimate object with default values @@ -459,12 +452,12 @@ public PoseEstimate() { this.tagSpan = 0; this.avgTagDist = 0; this.avgTagArea = 0; - this.rawFiducials = new RawFiducial[]{}; + this.rawFiducials = new RawFiducial[] {}; } - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials) { + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { this.pose = pose; this.timestampSeconds = timestampSeconds; @@ -492,22 +485,20 @@ static final String sanitizeName(String name) { return name; } - public static Pose3d toPose3D(double[] inData){ - if(inData.length < 6) - { - //System.err.println("Bad LL 3D Pose Data!"); + public static Pose3d toPose3D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); } - public static Pose2d toPose2D(double[] inData){ - if(inData.length < 6) - { - //System.err.println("Bad LL 2D Pose Data!"); + public static Pose2d toPose2D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -549,9 +540,8 @@ public static double[] pose2dToArray(Pose2d pose) { return result; } - private static double extractArrayEntry(double[] inData, int position){ - if(inData.length < position+1) - { + private static double extractArrayEntry(double[] inData, int position) { + if (inData.length < position + 1) { return 0; } return inData[position]; @@ -559,36 +549,36 @@ private static double extractArrayEntry(double[] inData, int position){ private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); - + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); double[] poseArray = tsValue.value; long timestamp = tsValue.timestamp; - + if (poseArray.length == 0) { // Handle the case where no data is available return null; // or some default PoseEstimate } - + var pose = toPose2D(poseArray); double latency = extractArrayEntry(poseArray, 6); - int tagCount = (int)extractArrayEntry(poseArray, 7); + int tagCount = (int) extractArrayEntry(poseArray, 7); double tagSpan = extractArrayEntry(poseArray, 8); double tagDist = extractArrayEntry(poseArray, 9); double tagArea = extractArrayEntry(poseArray, 10); - + // Convert server timestamp from microseconds to seconds and adjust for latency double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); - + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; int valsPerFiducial = 7; int expectedTotalVals = 11 + valsPerFiducial * tagCount; - + if (poseArray.length != expectedTotalVals) { // Don't populate fiducials } else { - for(int i = 0; i < tagCount; i++) { + for (int i = 0; i < tagCount; i++) { int baseIndex = 11 + (i * valsPerFiducial); - int id = (int)poseArray[baseIndex]; + int id = (int) poseArray[baseIndex]; double txnc = poseArray[baseIndex + 1]; double tync = poseArray[baseIndex + 2]; double ta = poseArray[baseIndex + 3]; @@ -598,7 +588,7 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); } } - + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); } @@ -609,10 +599,10 @@ public static RawDetection[] getRawDetections(String limelightName) { if (rawDetectionArray.length % valsPerEntry != 0) { return new RawDetection[0]; } - + int numDetections = rawDetectionArray.length / valsPerEntry; RawDetection[] rawDetections = new RawDetection[numDetections]; - + for (int i = 0; i < numDetections; i++) { int baseIndex = i * valsPerEntry; // Starting index for this detection's data int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); @@ -627,10 +617,11 @@ public static RawDetection[] getRawDetections(String limelightName) { double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); - - rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, + corner2_X, corner2_Y, corner3_X, corner3_Y); } - + return rawDetections; } @@ -639,7 +630,7 @@ public static void printPoseEstimate(PoseEstimate pose) { System.out.println("No PoseEstimate available."); return; } - + System.out.printf("Pose Estimate Information:%n"); System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); System.out.printf("Latency: %.3f ms%n", pose.latency); @@ -648,12 +639,12 @@ public static void printPoseEstimate(PoseEstimate pose) { System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); System.out.println(); - + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { System.out.println("No RawFiducials data available."); return; } - + System.out.println("Raw Fiducials Details:"); for (int i = 0; i < pose.rawFiducials.length; i++) { RawFiducial fiducial = pose.rawFiducials[i]; @@ -688,7 +679,7 @@ public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, St return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); }); } - + public static double getLimelightNTDouble(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); } @@ -705,8 +696,6 @@ public static double[] getLimelightNTDoubleArray(String tableName, String entryN return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); } - - public static String getLimelightNTString(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getString(""); } @@ -715,7 +704,6 @@ public static String[] getLimelightNTStringArray(String tableName, String entryN return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); } - public static URL getLimelightURLString(String tableName, String request) { String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; URL url; @@ -745,42 +733,39 @@ public static double getTA(String limelightName) { public static double[] getT2DArray(String limelightName) { return getLimelightNTDoubleArray(limelightName, "t2d"); } - public static int getTargetCount(String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[1]; - } - return 0; - } - - public static int getClassifierClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[10]; - } - return 0; - } - public static int getDetectorClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[11]; - } - return 0; - } - - public static String getClassifierClass (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[1]; + } + return 0; + } + + public static int getClassifierClassIndex(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[10]; + } + return 0; + } + + public static int getDetectorClassIndex(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[11]; + } + return 0; + } + + public static String getClassifierClass(String limelightName) { return getLimelightNTString(limelightName, "tcclass"); } - public static String getDetectorClass (String limelightName) { + + public static String getDetectorClass(String limelightName) { return getLimelightNTString(limelightName, "tdclass"); } - public static double getLatency_Pipeline(String limelightName) { return getLimelightNTDouble(limelightName, "tl"); } @@ -935,7 +920,8 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the BLUE * alliance * * @param limelightName @@ -946,7 +932,8 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the BLUE * alliance * * @param limelightName @@ -971,8 +958,10 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the RED * alliance + * * @param limelightName * @return */ @@ -981,8 +970,10 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the RED * alliance + * * @param limelightName * @return */ @@ -1015,7 +1006,6 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } - public static void setPriorityTagID(String limelightName, int ID) { setLimelightNTDouble(limelightName, "priorityid", ID); } @@ -1052,12 +1042,12 @@ public static void setStreamMode_PiPSecondary(String limelightName) { setLimelightNTDouble(limelightName, "stream", 2); } - /** * Sets the crop window. The crop window in the UI must be completely open for * dynamic cropping to work. */ - public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, + double cropYMax) { double[] entries = new double[4]; entries[0] = cropXMin; entries[1] = cropXMax; @@ -1065,7 +1055,7 @@ public static void setCropWindow(String limelightName, double cropXMin, double c entries[3] = cropYMax; setLimelightNTDoubleArray(limelightName, "crop", entries); } - + /** * Sets 3D offset point for easy 3D targeting. */ @@ -1077,21 +1067,21 @@ public static void setFiducial3DOffset(String limelightName, double offsetX, dou setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); } - public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); } - public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); } - private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate, boolean flush) { + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { double[] entries = new double[6]; entries[0] = yaw; @@ -1101,15 +1091,13 @@ private static void SetRobotOrientation_INTERNAL(String limelightName, double ya entries[4] = roll; entries[5] = rollRate; setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - if(flush) - { + if (flush) { Flush(); } } - - public static void SetFidcuial3DOffset(String limelightName, double x, double y, - double z) { + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { double[] entries = new double[3]; entries[0] = x; @@ -1122,37 +1110,32 @@ public static void SetFiducialIDFiltersOverride(String limelightName, int[] vali double[] validIDsDouble = new double[validIDs.length]; for (int i = 0; i < validIDs.length; i++) { validIDsDouble[i] = validIDs[i]; - } + } setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); } - public static void SetFiducialDownscalingOverride(String limelightName, float downscale) - { + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) { int d = 0; // pipeline - if (downscale == 1.0) - { + if (downscale == 1.0) { d = 1; } - if (downscale == 1.5) - { + if (downscale == 1.5) { d = 2; } - if (downscale == 2) - { + if (downscale == 2) { d = 3; } - if (downscale == 3) - { + if (downscale == 3) { d = 4; } - if (downscale == 4) - { + if (downscale == 4) { d = 5; } setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); } - - public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, + double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; entries[1] = side; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index efa081f..840f4cd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -139,7 +139,7 @@ public static final class SuperstructureConstants { public static final int GRABBER_UP_MOTOR_ID = 0; public static final int GRABBER_LOW_MOTOR_ID = 0; public static final int GRABBER_ANGLE_MOTOR_ID = 0; - + // grabber cancoder constants public static final int GRABBER_CANCODER_ID = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1260dcc..652d452 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,7 @@ import frc.robot.subsystems.Swerve; public class RobotContainer { - + public CommandXboxController controller = new CommandXboxController(RobotConstants.DRIVE_CONTROLLER_PORT); private final Swerve swerve = new Swerve(); diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index e426f25..33d091a 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -51,7 +51,7 @@ public void execute() { // square the input to inprove driving experience xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed); - ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed); + ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed); rotSpeed = Math.copySign(rotSpeed * rotSpeed, rotSpeed); swerve.drive( diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 58a7f2c..7b86440 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -34,9 +34,9 @@ public class Elevator extends SubsystemBase { // shuffleboard ShuffleboardTab tab = Shuffleboard.getTab("Elevator"); GenericEntry elevatorHeight = tab.add("Elevator Height", 0) - .withPosition(0, 0).withSize(2, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); + .withPosition(0, 0).withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); public Elevator() { leftElevatorMotor = new SparkMax(SuperstructureConstants.ELEVATOR_LEFT_MOTOR_ID, MotorType.kBrushless); diff --git a/src/main/java/frc/robot/subsystems/Grabber.java b/src/main/java/frc/robot/subsystems/Grabber.java index 9d43883..92b6c32 100644 --- a/src/main/java/frc/robot/subsystems/Grabber.java +++ b/src/main/java/frc/robot/subsystems/Grabber.java @@ -42,17 +42,17 @@ public class Grabber extends SubsystemBase { // shuffleboard ShuffleboardTab tab = Shuffleboard.getTab("Grabber"); GenericEntry upMotorSpeed = tab.add("Up Motor Speed (RPM)", 0) - .withPosition(0, 0).withSize(2, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); + .withPosition(0, 0).withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); GenericEntry lowMotorSpeed = tab.add("Low Motor Speed (RPM)", 0) - .withPosition(1, 0).withSize(2, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); + .withPosition(1, 0).withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); GenericEntry angle = tab.add("Grabber Angle (degrees)", 0) - .withPosition(2, 0) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); + .withPosition(2, 0) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); public Grabber() { upMotor = new SparkMax(SuperstructureConstants.GRABBER_UP_MOTOR_ID, MotorType.kBrushless); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index c6d8336..294f1f4 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -20,71 +20,71 @@ import frc.robot.Constants.SuperstructureConstants; public class Intake extends SubsystemBase { - /* - * a simple intake with a row of jelly wheels - * can pick up ALGAE from the floor - * it can help the robot climb - * when folded into the chassis with DEEP CAGE - * - * control jelly wheels by NEO 550 - * control its angle by NEO Brushless (gear ratio : 61.875) - */ - - // motor for jelly wheels - private final SparkMax intakeMotor; - - // motor for angle - private final SparkMax angleMotor; - - // cancoder - private final CANcoder cancoder; - - // shuffleboard - ShuffleboardTab tab = Shuffleboard.getTab("Intake"); - GenericEntry intakeSpeed = tab.add("Intake Speed (RPM)", 0) - .withPosition(0, 0).withSize(2, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); - GenericEntry intakeAngle = tab.add("Intake Angle (degrees)", 0) - .withPosition(1, 0).withSize(2, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); - - public Intake() { - intakeMotor = new SparkMax(SuperstructureConstants.INTAKE_MOTOR_ID, MotorType.kBrushless); - intakeMotor.configure(SuperstructureConstants.INTAKE_MOTOR_CONFIGURATION, ResetMode.kResetSafeParameters, - PersistMode.kNoPersistParameters); - angleMotor = new SparkMax(SuperstructureConstants.INTAKE_ANGLE_MOTOR_ID, MotorType.kBrushless); - angleMotor.configure(SuperstructureConstants.INTAKE_ANGLE_MOTOR_CONFIGURATION, ResetMode.kResetSafeParameters, - PersistMode.kNoPersistParameters); - - cancoder = new CANcoder(SuperstructureConstants.INTAKE_CANCODER_ID, RobotConstants.CANBUS_NAME); - cancoder.getConfigurator().apply(SuperstructureConstants.INTAKE_CANCODER_CONFIGURATION); - } - - public void setIntakeVolt(double volts) { - intakeMotor.setVoltage(volts); - } - - public void setIntakeSpeed(double speed) { - intakeMotor.set(speed); - } - - public void setAngleMotorVolt(double volts) { - angleMotor.setVoltage(volts); - } - - public double getIntakeSpeed() { - return intakeMotor.getEncoder().getVelocity(); - } - - public double getAngle() { - return Rotation2d.fromRotations(cancoder.getAbsolutePosition().getValueAsDouble()).getDegrees(); - } - - @Override - public void periodic() { - intakeSpeed.setDouble(getIntakeSpeed()); - intakeAngle.setDouble(getAngle()); - } + /* + * a simple intake with a row of jelly wheels + * can pick up ALGAE from the floor + * it can help the robot climb + * when folded into the chassis with DEEP CAGE + * + * control jelly wheels by NEO 550 + * control its angle by NEO Brushless (gear ratio : 61.875) + */ + + // motor for jelly wheels + private final SparkMax intakeMotor; + + // motor for angle + private final SparkMax angleMotor; + + // cancoder + private final CANcoder cancoder; + + // shuffleboard + ShuffleboardTab tab = Shuffleboard.getTab("Intake"); + GenericEntry intakeSpeed = tab.add("Intake Speed (RPM)", 0) + .withPosition(0, 0).withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + GenericEntry intakeAngle = tab.add("Intake Angle (degrees)", 0) + .withPosition(1, 0).withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + + public Intake() { + intakeMotor = new SparkMax(SuperstructureConstants.INTAKE_MOTOR_ID, MotorType.kBrushless); + intakeMotor.configure(SuperstructureConstants.INTAKE_MOTOR_CONFIGURATION, ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters); + angleMotor = new SparkMax(SuperstructureConstants.INTAKE_ANGLE_MOTOR_ID, MotorType.kBrushless); + angleMotor.configure(SuperstructureConstants.INTAKE_ANGLE_MOTOR_CONFIGURATION, ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters); + + cancoder = new CANcoder(SuperstructureConstants.INTAKE_CANCODER_ID, RobotConstants.CANBUS_NAME); + cancoder.getConfigurator().apply(SuperstructureConstants.INTAKE_CANCODER_CONFIGURATION); + } + + public void setIntakeVolt(double volts) { + intakeMotor.setVoltage(volts); + } + + public void setIntakeSpeed(double speed) { + intakeMotor.set(speed); + } + + public void setAngleMotorVolt(double volts) { + angleMotor.setVoltage(volts); + } + + public double getIntakeSpeed() { + return intakeMotor.getEncoder().getVelocity(); + } + + public double getAngle() { + return Rotation2d.fromRotations(cancoder.getAbsolutePosition().getValueAsDouble()).getDegrees(); + } + + @Override + public void periodic() { + intakeSpeed.setDouble(getIntakeSpeed()); + intakeAngle.setDouble(getAngle()); + } } diff --git a/src/main/java/frc/robot/subsystems/LED.java b/src/main/java/frc/robot/subsystems/LED.java index 5cd10dc..79019c2 100644 --- a/src/main/java/frc/robot/subsystems/LED.java +++ b/src/main/java/frc/robot/subsystems/LED.java @@ -7,11 +7,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LED extends SubsystemBase { - /** Creates a new LED. */ - public LED() {} + /** Creates a new LED. */ + public LED() { + } - @Override - public void periodic() { - // This method will be called once per scheduler run - } + @Override + public void periodic() { + // This method will be called once per scheduler run + } } diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 1343af6..5a8ea78 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -45,7 +45,8 @@ public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) { lastAngle = new Rotation2d(); steerMotor = new SparkMax(moduleConstants.SteerMotorId, MotorType.kBrushless); - steerMotor.configure(SwerveConstants.STEER_MOTOR_CONFIGURATION, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + steerMotor.configure(SwerveConstants.STEER_MOTOR_CONFIGURATION, ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters); driveMotor = new TalonFX(moduleConstants.DriveMotorId, RobotConstants.CANBUS_NAME); driveMotor.getConfigurator().apply(SwerveConstants.DRIVE_MOTOR_CONFIGURATION);