From: iefomit Date: Tue, 17 Feb 2026 19:38:13 +0000 (-0800) Subject: code cleanup X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=710390c82d90efc6fbc4be33742f2a03772db256;p=FRC2026.git code cleanup --- diff --git a/.vscode/settings.json b/.vscode/settings.json index 6f5f6cb..2af50e3 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,5 +58,4 @@ "edu.wpi.first.math.**.struct.*", ], "java.dependency.enableDependencyCheckup": false, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 180827f..54e3ec4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,7 +73,6 @@ public class RobotContainer { * Different robots may have different subsystems. */ public RobotContainer(RobotId robotId) { - // climb = new Climb(); // display the current robot id on smartdashboard SmartDashboard.putString("RobotID", robotId.toString()); @@ -94,21 +93,26 @@ public class RobotContainer { intake = new Intake(); climb = new Climb(); spindexer = new Spindexer(); + break; case WaffleHouse: // AKA Betabot turret = new Turret(); shooter = new Shooter(); - //hood = new Hood(); + // hood = new Hood(); + break; case SwerveCompetition: // AKA "Vantage" + break; case BetaBot: // AKA "Pancake" vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); - // fall-through + break; case Vivace: + break; case Phil: // AKA "IHOP" + break; case Vertigo: // AKA "French Toast" drive = new Drivetrain(vision, new GyroIOPigeon2()); @@ -117,11 +121,11 @@ public class RobotContainer { // Detected objects need access to the drivetrain DetectedObject.setDrive(drive); - + // SignalLogger.start(); driver.configureControls(); operator.configureControls(); - + initializeAutoBuilder(); registerCommands(); PathGroupLoader.loadPathGroups(); @@ -134,7 +138,7 @@ public class RobotContainer { } drive.setDefaultCommand(new DefaultDriveCommand(drive, driver)); break; - } + } // This is really annoying so it's disabled DriverStation.silenceJoystickConnectionWarning(true); @@ -155,7 +159,6 @@ public class RobotContainer { drive.setVisionEnabled(enabled); } - public void initializeAutoBuilder() { AutoBuilder.configure( () -> drive.getPose(), @@ -192,15 +195,14 @@ public class RobotContainer { } public boolean brownout() { - if(RobotController.getBatteryVoltage() < 6.0) { + if (RobotController.getBatteryVoltage() < 6.0) { return true; - } - else { + } else { return false; } } -// Autos + // Autos /** * Initialize the SendableChooser on the SmartDashbboard. @@ -219,6 +221,7 @@ public class RobotContainer { /** * Gets the auto command from SmartDashboard + * * @return */ public Command getAutoCommand() { @@ -228,16 +231,14 @@ public class RobotContainer { return autoSelected; } - public void logComponents(){ - if(!Constants.LOG_MECHANISMS) return; - + public void logComponents() { + if (!Constants.LOG_MECHANISMS) + return; + Logger.recordOutput( - "ComponentPoses", - new Pose3d[] { + "ComponentPoses", + new Pose3d[] { // Subsystem Pose3ds - } - ); + }); } } - - diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index 3ce8df2..7aa4cd5 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -34,6 +34,7 @@ public class AutoShootCommand extends Command { private Shooter shooter; private Spindexer spindexer; + // TODO: Implement state machine with WantedState/CurrentState private enum WantedState { IDLE, SHOOTING, @@ -51,12 +52,9 @@ public class AutoShootCommand extends Command { private WantedState wantedState = WantedState.IDLE; private CurrentState currentState = CurrentState.IDLE; - private void updateStates(){ - - } - - //TODO: find maximum interpolation - private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); + // TODO: find maximum interpolation + private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, + HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); private double turretSetpoint; private double hoodSetpoint; @@ -65,7 +63,7 @@ public class AutoShootCommand extends Command { private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); - + private Rotation2d lastTurretAngle; private Rotation2d turretAngle; private double turretVelocity; @@ -88,83 +86,94 @@ public class AutoShootCommand extends Command { this.hood = hood; this.shooter = shooter; this.spindexer = spindexer; - drivepose = drivetrain.getPose(); - //drivepose = new Pose2d(drivepose.getTranslation(), drivepose.getRotation().plus(new Rotation2d(Math.PI))); + drivepose = drivetrain.getPose(); + // drivepose = new Pose2d(drivepose.getTranslation(), + // drivepose.getRotation().plus(new Rotation2d(Math.PI))); goalState = ShooterPhysics.getShotParams( - FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), - 8.0); // Random initial goalState to prevent it being null - + FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), + 8.0); // Random initial goalState to prevent it being null + addRequirements(turret); } public void updateSetpoints(Pose2d drivepose) { - Pose2d turretPosition = drivepose.transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d())); + Pose2d turretPosition = drivepose + .transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(), + TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d())); double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); - + // If the robot is moving, adjust the target position based on velocity ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); - double turretVelocityX = - fieldRelVel.vxMetersPerSecond + double turretVelocityX = fieldRelVel.vxMetersPerSecond + fieldRelVel.omegaRadiansPerSecond - * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians()) - - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians())); - double turretVelocityY = - fieldRelVel.vyMetersPerSecond + * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() + * Math.cos(drivepose.getRotation().getRadians()) + - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() + * Math.sin(drivepose.getRotation().getRadians())); + double turretVelocityY = fieldRelVel.vyMetersPerSecond + fieldRelVel.omegaRadiansPerSecond - * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians()) - - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians())); + * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() + * Math.cos(drivepose.getRotation().getRadians()) + - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() + * Math.sin(drivepose.getRotation().getRadians())); double timeOfFlight; Pose2d lookaheadPose = turretPosition; - //double lookaheadTurretToTargetDistance = turretToTargetDistance; + // double lookaheadTurretToTargetDistance = turretToTargetDistance; /* * Loop (20) until lookaheadPose converges BECAUSE --> - * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s) - * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s + * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate + * for 6m (t = 0.8s) + * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was + * derived using t = 1.0s * So we make a bunch of guesses until it converges */ for (int i = 0; i < 20; i++) { - Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ()); - Translation3d target3d = new Translation3d(target.getX(), target.getY(), FieldConstants.getHubTranslation().getZ()); // Add if statement so that it's only when it's shooting + Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), + TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ()); + Translation3d target3d = new Translation3d(target.getX(), target.getY(), + FieldConstants.getHubTranslation().getZ()); // Add if statement so that it's only when it's shooting goalState = ShooterPhysics.getShotParams( - target3d.minus(lookahead3d), - 2.0); + target3d.minus(lookahead3d), + 2.0); timeOfFlight = goalState.timeOfFlight(); double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; - lookaheadPose = - new Pose2d( + lookaheadPose = new Pose2d( turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), turretPosition.getRotation()); - //lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); + // lookaheadTurretToTargetDistance = + // target.getDistance(lookaheadPose.getTranslation()); } turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); if (lastTurretAngle == null) { lastTurretAngle = turretAngle; } - turretVelocity = - turretAngleFilter.calculate( - turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); + turretVelocity = turretAngleFilter.calculate( + turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); lastTurretAngle = turretAngle; Logger.recordOutput("Lookahead Pose", lookaheadPose); - double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); + double adjustedTurretSetpoint = MathUtil + .angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); // Shortest path - double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180); + double error = MathUtil.inputModulus( + Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, + 180); double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error; - // Stay within +/- 200 -- if shortest path is past 200, we go long way around + // Stay within +/- 200 -- if shortest path is past 200, we go long way around double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE; - if (potentialSetpoint > turretRange/2) { + if (potentialSetpoint > turretRange / 2) { potentialSetpoint -= 360; - } else if (potentialSetpoint < -turretRange/2) { + } else if (potentialSetpoint < -turretRange / 2) { potentialSetpoint += 360; } @@ -172,7 +181,8 @@ public class AutoShootCommand extends Command { // Pitch is in radians hoodAngle = goalState.pitch(); - hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); + hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, + HoodConstants.MAX_ANGLE); hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); lastHoodAngle = hoodAngle; @@ -181,19 +191,19 @@ public class AutoShootCommand extends Command { } } - public void updateDrivePose(){ + public void updateDrivePose() { Pose2d currentPose = drivetrain.getPose(); // Add 180 degrees to the rotation bc robot is backwards drivepose = new Pose2d( - currentPose.getTranslation(), - currentPose.getRotation()//.plus(new Rotation2d(Math.PI)) + currentPose.getTranslation(), + currentPose.getRotation() ); ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); drivepose.exp( - new Twist2d( - robotRelVel.vxMetersPerSecond * phaseDelay, - robotRelVel.vyMetersPerSecond * phaseDelay, - robotRelVel.omegaRadiansPerSecond * phaseDelay)); + new Twist2d( + robotRelVel.vxMetersPerSecond * phaseDelay, + robotRelVel.vyMetersPerSecond * phaseDelay, + robotRelVel.omegaRadiansPerSecond * phaseDelay)); } @Override @@ -201,17 +211,18 @@ public class AutoShootCommand extends Command { updateDrivePose(); updateSetpoints(drivepose); - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); - //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity); - //shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), + turretVelocity - drivetrain.getAngularRate(2)); + // hood.setFieldRelativeTarget(new + // Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity); + // shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint); SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint); SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel()); - System.out.println("COMMAND IS WORKINNGGG"); /** Spindexer Stuff!! */ - if(spindexer != null){ + if (spindexer != null) { spindexer.maxSpindexer(); } } @@ -221,8 +232,9 @@ public class AutoShootCommand extends Command { // Set the turret to a safe position when the command ends turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); shooter.setShooter(0.0); - //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0); - if(spindexer != null){ + // hood.setFieldRelativeTarget(new + // Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0); + if (spindexer != null) { spindexer.stopSpindexer(); } } diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 59be846..d8ba7ce 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.1", + "version": "2026.0.2", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.1" + "version": "2026.0.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.1", + "version": "2026.0.2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.2", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.2", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -128,7 +128,6 @@ "linuxarm32", "osxuniversal" ] - } ] } \ No newline at end of file diff --git a/vendordeps/yams.json b/vendordeps/yams.json deleted file mode 100644 index 9557c14..0000000 --- a/vendordeps/yams.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "fileName": "yams.json", - "name": "Yet Another Mechanism System", - "version": "2026.2.3", - "frcYear": "2026", - "uuid": "a1051e86-a979-4880-a28b-a0d5362d1d96", - "mavenUrls": [ - "https://yet-another-software-suite.github.io/YAMS/releases/" - ], - "jsonUrl": "https://yet-another-software-suite.github.io/YAMS/yams.json", - "javaDependencies": [ - { - "groupId": "yams", - "artifactId": "YAMS-java", - "version": "2026.2.3" - } - ], - "cppDependencies": [], - "jniDependencies": [], - "requires": [] -} \ No newline at end of file