From 168b6f9643bc36fa7ad57c2bca07caab00bdd7ad Mon Sep 17 00:00:00 2001 From: Shikhar Date: Mon, 23 Feb 2026 17:33:25 -0700 Subject: [PATCH 1/6] auto stuf auto stuff --- .../pathplanner/autos/Auto Test Slow.auto | 19 ------- .../deploy/pathplanner/paths/Quick Shoot.path | 51 +++++++----------- src/main/deploy/pathplanner/paths/Taxi.path | 22 ++++---- .../deploy/pathplanner/paths/TurnNinety.path | 54 ------------------- .../java/frc4388/robot/RobotContainer.java | 11 +++- .../robot/constants/BuildConstants.java | 10 ++-- .../frc4388/robot/constants/Constants.java | 2 +- .../robot/subsystems/shooter/Shooter.java | 34 ++++++++---- 8 files changed, 70 insertions(+), 133 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Auto Test Slow.auto delete mode 100644 src/main/deploy/pathplanner/paths/TurnNinety.path diff --git a/src/main/deploy/pathplanner/autos/Auto Test Slow.auto b/src/main/deploy/pathplanner/autos/Auto Test Slow.auto deleted file mode 100644 index bb1c386..0000000 --- a/src/main/deploy/pathplanner/autos/Auto Test Slow.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TurnNinety" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Quick Shoot.path b/src/main/deploy/pathplanner/paths/Quick Shoot.path index 69333b6..11d6ce4 100644 --- a/src/main/deploy/pathplanner/paths/Quick Shoot.path +++ b/src/main/deploy/pathplanner/paths/Quick Shoot.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 3.48951566951567, + "x": 3.605428571428572, "y": 5.107378917378918 }, "prevControl": null, "nextControl": { - "x": 2.908105413105413, - "y": 6.205598290598291 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.616082621082621, + "x": 2.6073542083177803, "y": 5.107378917378918 }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2557261904761914, + "y": 4.801630952380952 + }, "prevControl": { - "x": 0.44392059766331804, - "y": 6.342550994319343 + "x": 2.5688571428571443, + "y": 5.4170952380952375 }, "nextControl": { - "x": 2.234130952380953, - "y": 4.456107142857142 + "x": 1.848588583599833, + "y": 4.00139496645156 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.8064047619047625, - "y": 3.8622380952380952 + "x": 2.2557261904761914, + "y": 4.035 }, "prevControl": { - "x": 2.8172023809523816, - "y": 4.132178571428572 + "x": 2.2665238095238105, + "y": 4.304940476190477 }, "nextControl": null, "isLocked": false, @@ -46,18 +46,7 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.612, - "y": 4.0213534 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.9140287769784197, - "maxWaypointRelativePos": 2, - "name": "Point Towards Zone" - } - ], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 0.5, @@ -69,7 +58,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -1.591140271194531 + "rotation": 180.0 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path index 4777d33..497847e 100644 --- a/src/main/deploy/pathplanner/paths/Taxi.path +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 3.62702380952381, + "y": 6.0 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 4.627023809523808, + "y": 6.0 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.9612559241706156, - "y": 7.0 + "x": 2.180142857142858, + "y": 6.0 }, "prevControl": { - "x": 1.9612559241706156, - "y": 7.0 + "x": 1.180142857142858, + "y": 6.0 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.2, + "maxVelocity": 1.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 178.89829388479367 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 179.95980050207663 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TurnNinety.path b/src/main/deploy/pathplanner/paths/TurnNinety.path deleted file mode 100644 index b8ac2c4..0000000 --- a/src/main/deploy/pathplanner/paths/TurnNinety.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.0, - "y": 4.0 - }, - "prevControl": null, - "nextControl": { - "x": 4.0, - "y": 4.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 4.0 - }, - "prevControl": { - "x": 2.0, - "y": 4.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 0.1, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c65108e..6fad0ca 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -100,6 +100,14 @@ public class RobotContainer { private Command RobotShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.setShooterReady()), + new RunCommand( + () -> { + m_robotSwerveDrive.driveFacingPosition( + getDeadbandedDriverController().getLeft(), + FieldPositions.HUB_POSITION, + ShooterConstants.AIM_LEAD_TIME.get() + ); + }, m_robotSwerveDrive), new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)), new WaitCommand(5), new InstantCommand(()->m_robotShooter.setShooterShoot()), @@ -262,8 +270,7 @@ public class RobotContainer { m_robotShooter.setShooterReadyFeeder(); })); - - + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index ebec1b4..7774220 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 75; - public static final String GIT_SHA = "4907e0c8a0fc7dade91b2075d70a2b38213f9cab"; - public static final String GIT_DATE = "2026-02-21 15:08:32 MST"; + public static final int GIT_REVISION = 79; + public static final String GIT_SHA = "ff0cff819cc7280a353d7ce86999efe16661f33b"; + public static final String GIT_DATE = "2026-02-23 16:58:14 MST"; public static final String GIT_BRANCH = "operator-controls"; - public static final String BUILD_DATE = "2026-02-21 15:52:02 MST"; - public static final long BUILD_UNIX_TIME = 1771714322479L; + public static final String BUILD_DATE = "2026-02-23 17:28:16 MST"; + public static final long BUILD_UNIX_TIME = 1771892896573L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 20e0a13..550c761 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -119,7 +119,7 @@ public final class Constants { public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE; - public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE; + // public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 31bde6b..4e1d572 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -126,8 +126,11 @@ public class Shooter extends SubsystemBase { boolean badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); // boolean intakeBad = m_Intake.getMode() == IntxakeMode.Extended; + boolean feedMdoe = this.mode == ShooterMode.ReadyFeeder | + this.mode == ShooterMode.ShootingFeeder; - int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (this.mode == ShooterMode.ReadyFeeder ? 4 : 0); + int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + ( + (feedMdoe) ? 4 : 0); switch (bitmask) { case 0b000: // No Errors m_robotLED.setMode(Constants.LEDConstants.OPREADY); @@ -147,19 +150,30 @@ public class Shooter extends SubsystemBase { break; case 0b100: - m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); - break; case 0b101: - m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED_BADPHYS); + m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); break; } // // We set the shooter mode to ready if there are no errors - mode = ( - bitmask == 0 ? - ShooterMode.Shooting : - ShooterMode.Ready - ); + + if (!feedMdoe) { + mode = ( + bitmask == 0 ? + ShooterMode.Shooting : + ShooterMode.Ready + ); + } else { + + if(bitmask == 0b100 | + bitmask == 0b101) { + mode = ShooterMode.ShootingFeeder; + + } else { + mode = ShooterMode.ReadyFeeder; + } + + } } else { m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); @@ -196,7 +210,7 @@ public class Shooter extends SubsystemBase { break; case ReadyFeeder: - io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_FEED_VELOCITY.get())); io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); break; From a96901379b44326ee4fb86e5b83b41309cbbbe5a Mon Sep 17 00:00:00 2001 From: Shikhar Date: Mon, 23 Feb 2026 19:41:12 -0700 Subject: [PATCH 2/6] Change Field Pos --- .../frc4388/robot/subsystems/shooter/ShooterConstants.java | 2 +- src/main/java/frc4388/utility/compute/FieldPositions.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index a61f36f..3f4c63d 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -21,7 +21,7 @@ public class ShooterConstants { // public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40); public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); - public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", 35); + public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35); public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); diff --git a/src/main/java/frc4388/utility/compute/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java index b9c8df0..d0b438c 100644 --- a/src/main/java/frc4388/utility/compute/FieldPositions.java +++ b/src/main/java/frc4388/utility/compute/FieldPositions.java @@ -5,8 +5,8 @@ import edu.wpi.first.math.geometry.Translation2d; public class FieldPositions { // public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); // public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); - public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534); - public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534); + public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534 + 0.3); + public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534 + 0.3); // We set the default position to one just in case it doesn't update From 17052c892b6092c95b15727d20b60ea2b684bc82 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 24 Feb 2026 10:20:21 -0700 Subject: [PATCH 3/6] Improve shooter --- .../robot/subsystems/shooter/Shooter.java | 197 ++++++++---------- 1 file changed, 91 insertions(+), 106 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 4e1d572..a92d5f8 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -8,6 +8,7 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; import frc4388.robot.subsystems.LED; @@ -50,31 +51,24 @@ public class Shooter extends SubsystemBase { public enum ShooterMode { - // Shooter is actively shooting Shooting, - // Shooter is going to fire soon - Ready, - - ShootingFeeder, - ReadyFeeder, - - // Not ready to shoot - NotReady, + Feeding, + Idle } - private ShooterMode mode = ShooterMode.NotReady; + private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void setShooterReady() { - this.mode = ShooterMode.Ready; + public void setShooterShooting() { + this.mode = ShooterMode.Shooting; } - public void setShooterReadyFeeder() { - this.mode = ShooterMode.ReadyFeeder; + public void setShooterFeeding() { + this.mode = ShooterMode.Feeding; } - public void setShooterNotReady() { - this.mode = ShooterMode.NotReady; + public void setShooterIdle() { + this.mode = ShooterMode.Idle; } @@ -86,6 +80,7 @@ public class Shooter extends SubsystemBase { shooterButtonReady = false; } + @AutoLogOutput public ShooterMode getMode() { return mode; @@ -111,114 +106,104 @@ public class Shooter extends SubsystemBase { Logger.recordOutput("Hub Dist", distanceToHub); - if(this.mode != ShooterMode.NotReady) { - // TODO: get if the robot is within the angle of the hub - boolean driverError = - // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | - // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | - distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | - distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); + boolean driverError = + // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | + // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | + distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | + distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); - double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; - double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2; - boolean badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); - // boolean intakeBad = m_Intake.getMode() == IntxakeMode.Extended; - - boolean feedMdoe = this.mode == ShooterMode.ReadyFeeder | - this.mode == ShooterMode.ShootingFeeder; - - int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + ( - (feedMdoe) ? 4 : 0); - switch (bitmask) { - case 0b000: // No Errors - m_robotLED.setMode(Constants.LEDConstants.OPREADY); - break; - case 0b001: // No op err, yes driver err - m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS); - break; - - case 0b010: - case 0b110: // Bad flywheel, no driver err - m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); - break; - - case 0b011: - case 0b111: // Bad flywheel, yes driver err - m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); - break; - - case 0b100: - case 0b101: - m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); - break; - } - - // // We set the shooter mode to ready if there are no errors - - if (!feedMdoe) { - mode = ( - bitmask == 0 ? - ShooterMode.Shooting : - ShooterMode.Ready - ); - } else { - - if(bitmask == 0b100 | - bitmask == 0b101) { - mode = ShooterMode.ShootingFeeder; - - } else { - mode = ShooterMode.ReadyFeeder; - } - - } - - } else { - m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); - - } - - + double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; + double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2; + boolean badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); switch (mode) { case Shooting: io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); - if(shooterButtonReady) { - io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); - } else { - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + int bitmask = ( + (shooterButtonReady ? 1 : 0) + + (badShooterVelocity ? 2 : 0) + + (driverError ? 4 : 0) + ); + + switch (bitmask) { + case 0b000: // No errors but button is not pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.OPREADY); + break; + + case 0b001: // No errors and shoot button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.OPREADY); + break; + + case 0b010: // Bad shooter velocity, button is not pressed + case 0b011: // Bad shooter velocty, button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); + break; + + case 0b100: // Driver error, button is not pressed + case 0b101: // Driver error, button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS); + break; + + case 0b110: // Driver error, bad shooter vel, button is not pressed + case 0b111: // Driver error, bad shooter vel, button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); + break; } break; - - case Ready: - io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); - break; - - case ShootingFeeder: + case Feeding: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_FEED_VELOCITY.get())); - - if(shooterButtonReady) { - io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); - } else { - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + + int bitmask2 = ( + (shooterButtonReady ? 1 : 0) + + (badShooterVelocity ? 2 : 0) + ); + + switch (bitmask2) { + case 0b000: // No errors but button is not pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); + break; + + case 0b001: // No errors and shoot button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); + break; + + case 0b010: // Bad shooter velocity, button is not pressed + case 0b011: // Bad shooter velocty, button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); + break; + + // case 0b100: // Driver error, button is not pressed + // case 0b101: // Driver error, button is pressed + // m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); + // io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + // break; + + // case 0b110: // Driver error, bad shooter vel, button is not pressed + // case 0b111: // Driver error, bad shooter vel, button is pressed + // m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); + // io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + // break; } - break; - case ReadyFeeder: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_FEED_VELOCITY.get())); - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); break; - - case NotReady: + case Idle: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); break; - } + } } } From 6e8078e0beecd15a99a2bd9bb9fbcf759e5e0459 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 24 Feb 2026 13:50:30 -0700 Subject: [PATCH 4/6] Add better feeder idle, shooter aim lead. --- .../java/frc4388/robot/RobotContainer.java | 16 +++---- .../robot/subsystems/shooter/Shooter.java | 47 ++++++++++++++----- .../subsystems/shooter/ShooterConstants.java | 8 +++- .../robot/subsystems/shooter/ShooterIO.java | 6 +++ .../robot/subsystems/shooter/ShooterReal.java | 29 ++++++++++++ 5 files changed, 83 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6fad0ca..5a49a3e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -99,7 +99,7 @@ public class RobotContainer { ); private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.setShooterReady()), + new InstantCommand(() -> m_robotShooter.spinUpShooting()), new RunCommand( () -> { m_robotSwerveDrive.driveFacingPosition( @@ -110,9 +110,9 @@ public class RobotContainer { }, m_robotSwerveDrive), new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)), new WaitCommand(5), - new InstantCommand(()->m_robotShooter.setShooterShoot()), + new InstantCommand(()->m_robotShooter.allowShooting()), new WaitCommand(10), - new InstantCommand(()->m_robotShooter.setShooterNOTShoot()) + new InstantCommand(()->m_robotShooter.denyShooting()) ); // private Command RobotShoot = new SequentialCommandGroup( @@ -256,18 +256,18 @@ public class RobotContainer { new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterNotReady(); + m_robotShooter.spinUpIdle(); })); new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterReady(); + m_robotShooter.spinUpShooting(); m_robotIntake.setMode(IntakeMode.Idle); })); new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterReadyFeeder(); + m_robotShooter.spinUpFeeding(); })); @@ -275,9 +275,9 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterShoot(); + m_robotShooter.allowShooting(); })).onFalse(new InstantCommand(() -> { - m_robotShooter.setShooterNOTShoot(); + m_robotShooter.denyShooting(); })); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index a92d5f8..4ea2aaa 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,6 @@ package frc4388.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -7,8 +8,10 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; import frc4388.robot.subsystems.LED; @@ -59,24 +62,24 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void setShooterShooting() { + public void spinUpShooting() { this.mode = ShooterMode.Shooting; } - public void setShooterFeeding() { + public void spinUpFeeding() { this.mode = ShooterMode.Feeding; } - public void setShooterIdle() { + public void spinUpIdle() { this.mode = ShooterMode.Idle; } - public void setShooterShoot() { + public void allowShooting() { shooterButtonReady = true; } - public void setShooterNOTShoot() { + public void denyShooting() { shooterButtonReady = false; } @@ -95,18 +98,30 @@ public class Shooter extends SubsystemBase { io.updateInputs(state); - ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds; - double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2)); - double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI)); + // Get robot positon and speeds + ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds; + double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2)); + double AngSpeed = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI)); Pose2d robotPose2d = m_SwerveDrive.getPose2d(); - // - double distanceToHub = (robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm()); + + + // Calculate aim lead + // Get the current speed of the robot + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + + // Calculate a point to aim ahead of the actual position. + Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation()); + + // Get the robot's aim distance to hub + double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm()); + //Center of hub to cameras in inches Logger.recordOutput("Hub Dist", distanceToHub); - - boolean driverError = // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | @@ -199,7 +214,13 @@ public class Shooter extends SubsystemBase { break; case Idle: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); + + io.setShooterCurrentLimitSpeed( + state, + ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get(), + Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), + RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get()) + ); io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); break; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 3f4c63d..89b3cb7 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -18,14 +18,18 @@ public class ShooterConstants { public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; public static final double INDEXER_GEAR_RATIO = 1.; - // public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40); public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); - public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35); + // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); + public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.3); + public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.); + public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); + public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); // Shoot mode tolerances diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 463cb4b..4fd3916 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -38,6 +38,12 @@ public interface ShooterIO { // public default void setShooterAngle(ShooterState state, Angle angle) {} // public default void setShooterPitch(ShooterState state, Angle angle) {} public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {} + public default void setShooterCurrentLimitSpeed( + ShooterState state, + double percentOutput, + Current currentLimit, + AngularVelocity targetVelocity + ) {} // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {} public default void setIndexerOutput(ShooterState state, double percentOutput) {} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 04fb459..6b14722 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -1,9 +1,13 @@ package frc4388.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; public class ShooterReal implements ShooterIO { @@ -54,6 +58,31 @@ public class ShooterReal implements ShooterIO { m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps)); } + @Override + public void setShooterCurrentLimitSpeed( + ShooterState state, + double percentOutput, + Current currentLimit, + AngularVelocity targetVelocity + ) { + state.motor1TargetVelocity = targetVelocity; + state.motor2TargetVelocity = targetVelocity; + + double current = Math.abs(state.motor1Current.in(Amps)) + Math.abs(state.motor2Current.in(Amps)); + double velocity = (Math.abs(state.motor1Velocity.in(RotationsPerSecond)) + Math.abs(state.motor2Velocity.in(RotationsPerSecond))) / 2; + + if( + Math.abs(currentLimit.in(Amps)) > current && + Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity + ) { + m_shooter1Motor.set(percentOutput); + m_shooter2Motor.set(percentOutput); + } else { + m_shooter1Motor.set(0); + m_shooter2Motor.set(0); + } + } + @Override public void setIndexerOutput(ShooterState state, double percentOutput) { state.indexerTargetOutput = percentOutput; From 3197a3dce03bf34c001ef4009de3e7d98a181536 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Tue, 24 Feb 2026 22:17:58 -0700 Subject: [PATCH 5/6] changes --- .../java/frc4388/robot/constants/BuildConstants.java | 12 ++++++------ .../frc4388/robot/subsystems/swerve/SwerveDrive.java | 6 ++++++ .../java/frc4388/utility/compute/FieldPositions.java | 2 +- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 7774220..aa439ce 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 79; - public static final String GIT_SHA = "ff0cff819cc7280a353d7ce86999efe16661f33b"; - public static final String GIT_DATE = "2026-02-23 16:58:14 MST"; - public static final String GIT_BRANCH = "operator-controls"; - public static final String BUILD_DATE = "2026-02-23 17:28:16 MST"; - public static final long BUILD_UNIX_TIME = 1771892896573L; + public static final int GIT_REVISION = 84; + public static final String GIT_SHA = "a09a039cb8a5bef1ee897d3307355d975eefd232"; + public static final String GIT_DATE = "2026-02-24 16:46:17 MST"; + public static final String GIT_BRANCH = "reveal-night"; + public static final String BUILD_DATE = "2026-02-24 18:42:57 MST"; + public static final long BUILD_UNIX_TIME = 1771983777848L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 228429e..2ee10d3 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems.swerve; +import static edu.wpi.first.units.Units.Rotations; + import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -245,6 +247,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + if(!TimesNegativeOne.isRed) { + leftStick.rotateBy(new Rotation2d(Math.PI/2.)); + } + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) diff --git a/src/main/java/frc4388/utility/compute/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java index d0b438c..f3a1f17 100644 --- a/src/main/java/frc4388/utility/compute/FieldPositions.java +++ b/src/main/java/frc4388/utility/compute/FieldPositions.java @@ -6,7 +6,7 @@ public class FieldPositions { // public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); // public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534 + 0.3); - public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534 + 0.3); + public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534);// + 0.3); // We set the default position to one just in case it doesn't update From 835d8779b059805ba2561234f11ef8d99ef8fb8f Mon Sep 17 00:00:00 2001 From: Shikhar Date: Wed, 25 Feb 2026 17:34:24 -0700 Subject: [PATCH 6/6] Fix shooter idle speed. --- .../robot/constants/BuildConstants.java | 10 +++---- .../robot/subsystems/shooter/Shooter.java | 6 ++-- .../subsystems/shooter/ShooterConstants.java | 4 +-- .../robot/subsystems/shooter/ShooterIO.java | 6 ++-- .../robot/subsystems/shooter/ShooterReal.java | 30 +++++++++---------- 5 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index aa439ce..8d8246e 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 84; - public static final String GIT_SHA = "a09a039cb8a5bef1ee897d3307355d975eefd232"; - public static final String GIT_DATE = "2026-02-24 16:46:17 MST"; + public static final int GIT_REVISION = 85; + public static final String GIT_SHA = "3197a3dce03bf34c001ef4009de3e7d98a181536"; + public static final String GIT_DATE = "2026-02-24 22:17:58 MST"; public static final String GIT_BRANCH = "reveal-night"; - public static final String BUILD_DATE = "2026-02-24 18:42:57 MST"; - public static final long BUILD_UNIX_TIME = 1771983777848L; + public static final String BUILD_DATE = "2026-02-25 16:32:13 MST"; + public static final long BUILD_UNIX_TIME = 1772062333884L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 4ea2aaa..a580a84 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -217,9 +217,9 @@ public class Shooter extends SubsystemBase { io.setShooterCurrentLimitSpeed( state, - ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get(), - Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), - RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get()) + ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get() + // Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), + // RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get()) ); io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 89b3cb7..c504b1d 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -23,8 +23,8 @@ public class ShooterConstants { // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.3); - public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.); - public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); + // public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.); + // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 4fd3916..c7d3369 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -40,9 +40,9 @@ public interface ShooterIO { public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {} public default void setShooterCurrentLimitSpeed( ShooterState state, - double percentOutput, - Current currentLimit, - AngularVelocity targetVelocity + double percentOutput + // Current currentLimit, + // AngularVelocity targetVelocity ) {} // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {} public default void setIndexerOutput(ShooterState state, double percentOutput) {} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 6b14722..9a71787 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -61,26 +61,26 @@ public class ShooterReal implements ShooterIO { @Override public void setShooterCurrentLimitSpeed( ShooterState state, - double percentOutput, - Current currentLimit, - AngularVelocity targetVelocity + double percentOutput + // Current currentLimit, + // AngularVelocity targetVelocity ) { - state.motor1TargetVelocity = targetVelocity; - state.motor2TargetVelocity = targetVelocity; + // state.motor1TargetVelocity = targetVelocity; + // state.motor2TargetVelocity = targetVelocity; - double current = Math.abs(state.motor1Current.in(Amps)) + Math.abs(state.motor2Current.in(Amps)); - double velocity = (Math.abs(state.motor1Velocity.in(RotationsPerSecond)) + Math.abs(state.motor2Velocity.in(RotationsPerSecond))) / 2; + // double current = Math.abs(state.motor1Current.in(Amps)) + Math.abs(state.motor2Current.in(Amps)); + // double velocity = (Math.abs(state.motor1Velocity.in(RotationsPerSecond)) + Math.abs(state.motor2Velocity.in(RotationsPerSecond))) / 2; - if( - Math.abs(currentLimit.in(Amps)) > current && - Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity - ) { + // if( + // Math.abs(currentLimit.in(Amps)) > current && + // Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity + // ) { m_shooter1Motor.set(percentOutput); m_shooter2Motor.set(percentOutput); - } else { - m_shooter1Motor.set(0); - m_shooter2Motor.set(0); - } + // } else { + // m_shooter1Motor.set(0); + // m_shooter2Motor.set(0); + // } } @Override