From 5f87701583c3734d4b6c6d7f00d4bf53bd0e1fb9 Mon Sep 17 00:00:00 2001 From: Anshu Mukherjee Date: Sat, 14 Feb 2026 13:42:48 -0800 Subject: [PATCH 1/4] Added Aim and Shoot Command --- src/main/java/frc/robot/RobotContainer.java | 22 ++- .../frc/robot/subsystems/shooter/Shooter.java | 75 +++++++++- .../subsystems/shooter/ShooterConstants.java | 130 +++++++++++++++--- .../robot/subsystems/shooter/ShooterIO.java | 5 - 4 files changed, 203 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c1e16f7..3b9c82b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,7 +20,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -49,6 +52,7 @@ import frc.robot.subsystems.shooter.HoodIOServo; import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOTalonFX; @@ -315,11 +319,27 @@ private void configureBindings() { intake_::isIntakeStowed )); + + // While the left trigger is held, we will run the intake. If the intake is stowed, it will also deploy it. gamepad_.leftTrigger().whileTrue(intake_.intakeSequence()); // While the right trigger is held, we will shoot into the hub. - gamepad_.rightTrigger().whileTrue(shooter_.shootCmd(hopper_)); + gamepad_.rightTrigger().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_, + () -> 0, + () -> 0, + () -> { + Translation2d hub = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose; + + var hubTranslation = drivebase_.getPose().getTranslation().minus(hub); + var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); + + return rotation; + }), + shooter_.shooterSetpointSupplier(() -> drivebase_.getPose(), hopper_))); // When the hopper isnt shooting, set it to run its idle velocity. hopper_.setDefaultCommand(hopper_.idleScrambler()); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fbb17de..7fe866a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -15,12 +15,15 @@ import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -29,6 +32,7 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.subsystems.hopper.Hopper; +import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; @@ -99,8 +103,7 @@ public boolean isShooterReady() { public Current getShooterCurrent() { return (shooterInputs.shooter1Current) .plus(shooterInputs.shooter2Current) - .plus(shooterInputs.shooter3Current) - .plus(shooterInputs.shooter4Current); + .plus(shooterInputs.shooter3Current); } private void setHoodAngle(Angle pos) { @@ -142,7 +145,25 @@ public Command awaitShooting(Supplier robotPose) { return Degrees.zero(); } - return Degrees.of(45); // TODO: replace this with whatever determines shooter angle + Translation2d hubTranslation = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose; + + Distance distanceToHub = Meters.of(pose.getTranslation().getDistance(hubTranslation)); + + switch(HubDistance.fromDistance(distanceToHub)) { + case LOW: + return Degrees.of(ShooterConstants.Positions.hoodLOW); + case MEDIUM: + return Degrees.of(ShooterConstants.Positions.hoodMEDIUM); + case HIGH: + return Degrees.of(ShooterConstants.Positions.hoodHIGH); + default: + return Degrees.of(ShooterConstants.Positions.hoodLOW); + + } + }); } @@ -168,6 +189,54 @@ public Command hoodToPosCmd(Angle pos) { return runOnce(() -> setHoodAngle(pos)).withName("Set Hood Position"); } + /** + * Calculates Velocity and Hood Angle based on distance and Shoots + * + * + */ + public Command shooterSetpointSupplier(Supplier pose, Hopper hopper) { + + return Commands.parallel( + defer(() -> { + + // constructing + Translation2d hubTranslation = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose; + + ShooterConstants.Positions.initMap(); + + return runDynamicSetpoints(() -> { + Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); + var vel = ShooterConstants.Positions.distMap.get(distanceToHub.baseUnitMagnitude()); + + return RotationsPerSecond.of(vel); + }, + + () -> { + // periodic + Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); + + switch(HubDistance.fromDistance(distanceToHub)) { + case LOW: + return Degrees.of(ShooterConstants.Positions.hoodLOW); + + case MEDIUM: + return Degrees.of(ShooterConstants.Positions.hoodMEDIUM); + + case HIGH: + return Degrees.of(ShooterConstants.Positions.hoodHIGH); + + default: + return Degrees.of(ShooterConstants.Positions.hoodMEDIUM); + } + }); + }), + hopper.forwardFeed() + ); + } + /** * Runs supplied setpoints until the command ends, then stops. * @param vel diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index bcc0216..b9d97dc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -4,6 +4,8 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; @@ -24,31 +26,119 @@ public class ShooterConstants { public static final Distance allowedTrenchDistance = Meters.of(1.0); public class PID { - // shooter - public static final double shooterkP = 0.0; - public static final double shooterkI = 0.0; - public static final double shooterkD = 0.0; - public static final double shooterkV = 0.117; - public static final double shooterkA = 0.0; - public static final double shooterkG = 0.0; - public static final double shooterkS = 0.0; - } + // shooter + public static final double shooterkP = 0.5; + public static final double shooterkI = 0.0; + public static final double shooterkD = 0.0; + public static final double shooterkV = 0.0; + public static final double shooterkA = 0.0; + public static final double shooterkG = 0.0; + public static final double shooterkS = 0.0; - public class MotionMagic { + // hood + public static final double hoodkP = 0.0; + public static final double hoodkI = 0.0; + public static final double hoodkD = 0.0; + public static final double hoodkV = 0.0; + public static final double hoodkA = 0.0; + public static final double hoodkG = 0.0; + public static final double hoodkS = 0.0; + } - // shooter - public static final double shooterkMaxVelocity = 1000.0; - public static final double shooterkMaxAcceleration = 3000.0; - public static final double shooterkJerk = 0.0; - } + public class MotionMagic { + + // shooter + public static final double shooterkMaxVelocity = 0.0; + public static final double shooterkMaxAcceleration = 300.0; + public static final double shooterkJerk = 0.0; + + // hood + public static final double hoodkMaxVelocity = 0.0; + public static final double hoodkMaxAcceleration = 300.0; + public static final double hoodkJerk = 0.0; + } + + public class SoftwareLimits { + public static final double hoodMaxAngle = 0.0; + public static final double hoodMinAngle = 0.0; + } + + public class Positions { + public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536); + public static final Translation2d redHubPose = new Translation2d(11.938,4.034536); + + // Hood Setpoints + public static final double hoodLOW = 0; + public static final double hoodMEDIUM = 1; + public static final double hoodHIGH = 2; + + // Tested Distances + public static final double dist1 = 1; + public static final double dist2 = 2; + public static final double dist3 = 3; + + public static final double dist4 = 4; + public static final double dist5 = 5; + public static final double dist6 = 6; + + public static final double dist7 = 7; + public static final double dist8 = 8; + public static final double dist9 = 9; - public class SoftwareLimits { - public static final double hoodMaxAngle = 0.0; - public static final double hoodMinAngle = 0.0; + // Tested Velocities + public static final double low1 = 10; + public static final double low2 = 20; + public static final double low3 = 30; + + public static final double med1 = 40; + public static final double med2 = 50; + public static final double med3 = 60; + + public static final double high1 = 70; + public static final double high2 = 80; + public static final double high3 = 90; + + public static final InterpolatingDoubleTreeMap distMap = new InterpolatingDoubleTreeMap(); + + public static void initMap() { + distMap.put(dist1, low1); + distMap.put(dist2, low2); + distMap.put(dist3, low3); + distMap.put(dist4, med1); + distMap.put(dist5, med2); + distMap.put(dist6, med3); + distMap.put(dist7, high1); + distMap.put(dist8, high2); + distMap.put(dist9, high3); + } + + public enum HubDistance { + LOW(Meters.of(2)), // 0-2 m + MEDIUM(Meters.of(3)), // in + HIGH(Meters.of(4)); // 96+ in + + private final Distance maxDistance; + + private HubDistance(Distance maxDistance) { + this.maxDistance = maxDistance; + } + + public Distance maxDistance() { return maxDistance; } + + public static HubDistance fromDistance(Distance pos) { + for(HubDistance vol : values()) { + if (pos.baseUnitMagnitude() <= vol.maxDistance().baseUnitMagnitude()) { + return vol; + } + } + return HubDistance.HIGH; + } + } } public class HoodPWMs { - public static final int hoodLeftPWMPort = 2; - public static final int hoodRightPWMPort = 0; + public static final int hoodLeftPWMPort = 2; + public static final int hoodRightPWMPort = 0; } + } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 2bd953b..2ab5fbd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -29,11 +29,6 @@ public static class ShooterIOInputs { public Current shooter3Current = Amps.zero(); public AngularVelocity shooter3Velocity = RadiansPerSecond.zero(); - public boolean shooter4Ready = false; - public Voltage shooter4Voltage = Volts.zero(); - public Current shooter4Current = Amps.zero(); - public AngularVelocity shooter4Velocity = RadiansPerSecond.zero(); - public AngularVelocity wheelVelocity = RadiansPerSecond.zero(); } From bc0ab29aacddbf9448b7a20ad682cb5582eb31da Mon Sep 17 00:00:00 2001 From: Anshu Mukherjee Date: Sat, 14 Feb 2026 13:42:48 -0800 Subject: [PATCH 2/4] Added Aim and Shoot Command --- src/main/java/frc/robot/RobotContainer.java | 22 ++- .../frc/robot/subsystems/shooter/Shooter.java | 75 +++++++++- .../subsystems/shooter/ShooterConstants.java | 130 +++++++++++++++--- .../robot/subsystems/shooter/ShooterIO.java | 5 - 4 files changed, 203 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c1e16f7..3b9c82b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,7 +20,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -49,6 +52,7 @@ import frc.robot.subsystems.shooter.HoodIOServo; import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOTalonFX; @@ -315,11 +319,27 @@ private void configureBindings() { intake_::isIntakeStowed )); + + // While the left trigger is held, we will run the intake. If the intake is stowed, it will also deploy it. gamepad_.leftTrigger().whileTrue(intake_.intakeSequence()); // While the right trigger is held, we will shoot into the hub. - gamepad_.rightTrigger().whileTrue(shooter_.shootCmd(hopper_)); + gamepad_.rightTrigger().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_, + () -> 0, + () -> 0, + () -> { + Translation2d hub = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose; + + var hubTranslation = drivebase_.getPose().getTranslation().minus(hub); + var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); + + return rotation; + }), + shooter_.shooterSetpointSupplier(() -> drivebase_.getPose(), hopper_))); // When the hopper isnt shooting, set it to run its idle velocity. hopper_.setDefaultCommand(hopper_.idleScrambler()); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fbb17de..7fe866a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -15,12 +15,15 @@ import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -29,6 +32,7 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.subsystems.hopper.Hopper; +import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; @@ -99,8 +103,7 @@ public boolean isShooterReady() { public Current getShooterCurrent() { return (shooterInputs.shooter1Current) .plus(shooterInputs.shooter2Current) - .plus(shooterInputs.shooter3Current) - .plus(shooterInputs.shooter4Current); + .plus(shooterInputs.shooter3Current); } private void setHoodAngle(Angle pos) { @@ -142,7 +145,25 @@ public Command awaitShooting(Supplier robotPose) { return Degrees.zero(); } - return Degrees.of(45); // TODO: replace this with whatever determines shooter angle + Translation2d hubTranslation = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose; + + Distance distanceToHub = Meters.of(pose.getTranslation().getDistance(hubTranslation)); + + switch(HubDistance.fromDistance(distanceToHub)) { + case LOW: + return Degrees.of(ShooterConstants.Positions.hoodLOW); + case MEDIUM: + return Degrees.of(ShooterConstants.Positions.hoodMEDIUM); + case HIGH: + return Degrees.of(ShooterConstants.Positions.hoodHIGH); + default: + return Degrees.of(ShooterConstants.Positions.hoodLOW); + + } + }); } @@ -168,6 +189,54 @@ public Command hoodToPosCmd(Angle pos) { return runOnce(() -> setHoodAngle(pos)).withName("Set Hood Position"); } + /** + * Calculates Velocity and Hood Angle based on distance and Shoots + * + * + */ + public Command shooterSetpointSupplier(Supplier pose, Hopper hopper) { + + return Commands.parallel( + defer(() -> { + + // constructing + Translation2d hubTranslation = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose; + + ShooterConstants.Positions.initMap(); + + return runDynamicSetpoints(() -> { + Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); + var vel = ShooterConstants.Positions.distMap.get(distanceToHub.baseUnitMagnitude()); + + return RotationsPerSecond.of(vel); + }, + + () -> { + // periodic + Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); + + switch(HubDistance.fromDistance(distanceToHub)) { + case LOW: + return Degrees.of(ShooterConstants.Positions.hoodLOW); + + case MEDIUM: + return Degrees.of(ShooterConstants.Positions.hoodMEDIUM); + + case HIGH: + return Degrees.of(ShooterConstants.Positions.hoodHIGH); + + default: + return Degrees.of(ShooterConstants.Positions.hoodMEDIUM); + } + }); + }), + hopper.forwardFeed() + ); + } + /** * Runs supplied setpoints until the command ends, then stops. * @param vel diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index bcc0216..b9d97dc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -4,6 +4,8 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; @@ -24,31 +26,119 @@ public class ShooterConstants { public static final Distance allowedTrenchDistance = Meters.of(1.0); public class PID { - // shooter - public static final double shooterkP = 0.0; - public static final double shooterkI = 0.0; - public static final double shooterkD = 0.0; - public static final double shooterkV = 0.117; - public static final double shooterkA = 0.0; - public static final double shooterkG = 0.0; - public static final double shooterkS = 0.0; - } + // shooter + public static final double shooterkP = 0.5; + public static final double shooterkI = 0.0; + public static final double shooterkD = 0.0; + public static final double shooterkV = 0.0; + public static final double shooterkA = 0.0; + public static final double shooterkG = 0.0; + public static final double shooterkS = 0.0; - public class MotionMagic { + // hood + public static final double hoodkP = 0.0; + public static final double hoodkI = 0.0; + public static final double hoodkD = 0.0; + public static final double hoodkV = 0.0; + public static final double hoodkA = 0.0; + public static final double hoodkG = 0.0; + public static final double hoodkS = 0.0; + } - // shooter - public static final double shooterkMaxVelocity = 1000.0; - public static final double shooterkMaxAcceleration = 3000.0; - public static final double shooterkJerk = 0.0; - } + public class MotionMagic { + + // shooter + public static final double shooterkMaxVelocity = 0.0; + public static final double shooterkMaxAcceleration = 300.0; + public static final double shooterkJerk = 0.0; + + // hood + public static final double hoodkMaxVelocity = 0.0; + public static final double hoodkMaxAcceleration = 300.0; + public static final double hoodkJerk = 0.0; + } + + public class SoftwareLimits { + public static final double hoodMaxAngle = 0.0; + public static final double hoodMinAngle = 0.0; + } + + public class Positions { + public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536); + public static final Translation2d redHubPose = new Translation2d(11.938,4.034536); + + // Hood Setpoints + public static final double hoodLOW = 0; + public static final double hoodMEDIUM = 1; + public static final double hoodHIGH = 2; + + // Tested Distances + public static final double dist1 = 1; + public static final double dist2 = 2; + public static final double dist3 = 3; + + public static final double dist4 = 4; + public static final double dist5 = 5; + public static final double dist6 = 6; + + public static final double dist7 = 7; + public static final double dist8 = 8; + public static final double dist9 = 9; - public class SoftwareLimits { - public static final double hoodMaxAngle = 0.0; - public static final double hoodMinAngle = 0.0; + // Tested Velocities + public static final double low1 = 10; + public static final double low2 = 20; + public static final double low3 = 30; + + public static final double med1 = 40; + public static final double med2 = 50; + public static final double med3 = 60; + + public static final double high1 = 70; + public static final double high2 = 80; + public static final double high3 = 90; + + public static final InterpolatingDoubleTreeMap distMap = new InterpolatingDoubleTreeMap(); + + public static void initMap() { + distMap.put(dist1, low1); + distMap.put(dist2, low2); + distMap.put(dist3, low3); + distMap.put(dist4, med1); + distMap.put(dist5, med2); + distMap.put(dist6, med3); + distMap.put(dist7, high1); + distMap.put(dist8, high2); + distMap.put(dist9, high3); + } + + public enum HubDistance { + LOW(Meters.of(2)), // 0-2 m + MEDIUM(Meters.of(3)), // in + HIGH(Meters.of(4)); // 96+ in + + private final Distance maxDistance; + + private HubDistance(Distance maxDistance) { + this.maxDistance = maxDistance; + } + + public Distance maxDistance() { return maxDistance; } + + public static HubDistance fromDistance(Distance pos) { + for(HubDistance vol : values()) { + if (pos.baseUnitMagnitude() <= vol.maxDistance().baseUnitMagnitude()) { + return vol; + } + } + return HubDistance.HIGH; + } + } } public class HoodPWMs { - public static final int hoodLeftPWMPort = 2; - public static final int hoodRightPWMPort = 0; + public static final int hoodLeftPWMPort = 2; + public static final int hoodRightPWMPort = 0; } + } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 2bd953b..2ab5fbd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -29,11 +29,6 @@ public static class ShooterIOInputs { public Current shooter3Current = Amps.zero(); public AngularVelocity shooter3Velocity = RadiansPerSecond.zero(); - public boolean shooter4Ready = false; - public Voltage shooter4Voltage = Volts.zero(); - public Current shooter4Current = Amps.zero(); - public AngularVelocity shooter4Velocity = RadiansPerSecond.zero(); - public AngularVelocity wheelVelocity = RadiansPerSecond.zero(); } From 6b0349a1f754fea13291a6d691d095257bde42d3 Mon Sep 17 00:00:00 2001 From: Anshu Mukherjee Date: Sat, 14 Feb 2026 22:51:04 -0800 Subject: [PATCH 3/4] Fixed Aiming Orientation and Constants --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3b9c82b..5aaae06 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -334,7 +334,7 @@ private void configureBindings() { ? ShooterConstants.Positions.blueHubPose : ShooterConstants.Positions.redHubPose; - var hubTranslation = drivebase_.getPose().getTranslation().minus(hub); + var hubTranslation = hub.minus(drivebase_.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); return rotation; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index b9d97dc..11137a1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -48,8 +48,8 @@ public class PID { public class MotionMagic { // shooter - public static final double shooterkMaxVelocity = 0.0; - public static final double shooterkMaxAcceleration = 300.0; + public static final double shooterkMaxVelocity = 1000.0; + public static final double shooterkMaxAcceleration = 3000.0; public static final double shooterkJerk = 0.0; // hood From 0f3dcee680789d69569198c3ed2cfe81f9cb938d Mon Sep 17 00:00:00 2001 From: Anshu Mukherjee Date: Sat, 14 Feb 2026 22:53:07 -0800 Subject: [PATCH 4/4] Added Trench to Trench Auto --- .../java/frc/robot/commands/AutoCommands.java | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 src/main/java/frc/robot/commands/AutoCommands.java diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java new file mode 100644 index 0000000..930f1f9 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -0,0 +1,28 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.drive.DriveCommands; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.hopper.Hopper; +import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.shooter.Shooter; + +public class AutoCommands { + + + public static Command NeutralZoneTrenchToTrench(Drive drive, IntakeSubsystem intake, Hopper hopper, Shooter shooter){ + return Commands.sequence( + Commands.runOnce(() -> drive.resetGyroCmd()), + + DriveCommands.initialFollowPathCommand(drive,"TopToBottom Trench").deadlineFor(intake.intakeSequence()), + + shooter.shootCmd(hopper).withTimeout(3.8), + + DriveCommands.followPathCommand("BottomToTop").deadlineFor(intake.intakeSequence()), + + shooter.shootCmd(hopper).withTimeout(2.5) + ); + } + +} \ No newline at end of file