From 983b95fdc704ef35b6f5e2dada2c6348f3c67190 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Tue, 10 Feb 2026 18:42:47 -0800 Subject: [PATCH] IDk --- .../java/frc4388/robot/RobotContainer.java | 61 +++++++----- .../robot/subsystems/intake/Intake.java | 4 + .../robot/subsystems/intake/IntakeReal.java | 1 + .../robot/subsystems/shooter/Shooter.java | 93 ++++++++++++------- .../subsystems/shooter/ShooterConstants.java | 17 +++- .../robot/subsystems/shooter/ShooterReal.java | 45 --------- 6 files changed, 118 insertions(+), 103 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b322e41..2682829 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -65,7 +65,7 @@ public class RobotContainer { public final Vision m_vision = new Vision(); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO); - public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO); + public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED); private Boolean operatorManualMode = false; // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -175,15 +175,15 @@ public class RobotContainer { - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Extended);}, m_robotIntake)); - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> {;}, m_robotIntake)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); @@ -196,18 +196,7 @@ public class RobotContainer { m_robotShooter.io.updateGains(); })); - // IF the driver is holding the aim button, aim the robot towards the hub - new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( - () -> { - m_robotSwerveDrive.driveFacingPosition( - getDeadbandedDriverController().getLeft(), - FieldConstants.BLUE_HUB_POS); - }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - - - // IF the driver is holding the aim button, aim the robot towards the hub + // IF the driver is holding the left trigger, intake driving new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) .whileTrue(new RunCommand( () -> { @@ -216,7 +205,35 @@ public class RobotContainer { false ); }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + // .onFalse(new InstantCommand(() -> , m_robotSwerveDrive)); + + + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Extended); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracted); + m_robotSwerveDrive.softStop(); + }, m_robotSwerveDrive)); + + // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready + new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) + .whileTrue(new RunCommand( + () -> { + m_robotSwerveDrive.driveFacingPosition( + getDeadbandedDriverController().getLeft(), + FieldConstants.BLUE_HUB_POS); + }, m_robotSwerveDrive) + ) + .onTrue(new InstantCommand(() -> { + // When Right trigger is pressed, + m_robotIntake.setMode(IntakeMode.Extended); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracted); + m_robotSwerveDrive.softStop(); + }, m_robotSwerveDrive)); + // D-PAD fine alignment new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index d83292a..be507b3 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -39,6 +39,10 @@ public class Intake extends SubsystemBase { this.mode = mode; } + public IntakeMode getMode() { + return mode; + } + // public enum FieldZone { // // The robot should aim at the hub diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 598fa91..547cc90 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -78,6 +78,7 @@ public class IntakeReal implements IntakeIO { // Assume that the angle is always accurate, because I think we will use a shaft encoder // Assume that 0 degrees = forwards. Might need an offset here + // angle = clampAng(angle, IntakeConstants.ARM_LIMIT_RETRACTED, IntakeConstants.ARM_LIMIT_EXTENDED); // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 49ab73e..35e2501 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -2,23 +2,38 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RotationsPerSecond; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.intake.Intake.IntakeMode; +import frc4388.robot.subsystems.swerve.SwerveDrive; public class Shooter extends SubsystemBase { public ShooterIO io; ShooterStateAutoLogged state = new ShooterStateAutoLogged(); + SwerveDrive m_SwerveDrive; + Intake m_Intake; + LED m_robotLED; + + // Supplier m_swervePoseSupplier; public Shooter( - ShooterIO io - // Supplier swervePoseSupplier + ShooterIO io, + SwerveDrive swerveDrive, + Intake intake, + LED robotLED ) { this.io = io; + this.m_SwerveDrive = swerveDrive; + this.m_Intake = intake; + this.m_robotLED = robotLED; // this.m_swervePoseSupplier = swervePoseSupplier; } @@ -32,57 +47,67 @@ public class Shooter extends SubsystemBase { public enum ShooterMode { - //Shooter is at speed it fires balls - Active, - //Shooter is at a resting velocity - Resting, - //Shooter is inactive (Off) - Inactive, + // Shooter is actively shooting + Shooting, + // Shooter is going to fire soon + Ready, + // Not ready to shoot + NotReady, } - private ShooterMode mode = ShooterMode.Inactive; + private ShooterMode mode = ShooterMode.NotReady; - public void setMode(ShooterMode mode) { - this.mode = mode; + // public void setMode(ShooterMode mode) { + // this.mode = mode; + // } + + public void setShooterReady() { + if(this.mode == ShooterMode.NotReady) { + this.mode = ShooterMode.Ready; + } + } + + public void setShooterNotReady() { + this.mode = ShooterMode.NotReady; } - // Calculate what should be done based off of the position of the robot - // TODO: Implement field zones - public FieldZone getTarget(Pose2d position) { - return FieldZone.InShootZone; + @AutoLogOutput + public ShooterMode getMode() { + return mode; } + @Override public void periodic() { - - - // FaultReporter.register(this); // TODO Implement fault reporter - Logger.processInputs("Shooter", state); - - // Pose2d pose = m_swervePoseSupplier.get(); - // Angle robotRot = pose.getRotation().getMeasure(); - io.updateInputs(state); + if(this.mode != ShooterMode.NotReady) { + double badRobotSpeed = Math.sqrt(Math.pow(m_SwerveDrive.chassisSpeeds.vxMetersPerSecond,2) + Math.pow(m_SwerveDrive.chassisSpeeds.vyMetersPerSecond,2)); + double badAngSpeed = Math.abs(m_SwerveDrive.chassisSpeeds.omegaRadiansPerSecond); + double badShooterVelocity = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; + boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended; + + + + // TODO: get if the robot is within the correct distance of the hub + } + switch (mode) { - case Active: + case Shooting: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_FORWARD_VELOCITY.get())); break; - case Resting: + case Ready: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get())); + break; + case NotReady: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(0)); - break; - case Inactive: - io.setShooterVelocity(state, RotationsPerSecond.of(0)); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(0)); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get())); break; } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 81deb0e..531ed12 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -33,8 +33,21 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); // public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); - public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Indexer Active Velocity", 10); - // public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); + public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10); + public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10); + + // Tolerances + public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist", 1); + public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist", 1); + + public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Shoot Ang tolerance", 1); + + public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Shoot speed tolerance", 1); + public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance", 1); + + public static final ConfigurableDouble SHOOT_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance", 1); + + public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 928b09d..34dc2df 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -43,52 +43,7 @@ public class ShooterReal implements ShooterIO { shooter2Velocity.Slot = 0; m_indexerVelocity.Slot = 0; } - - private Angle clampAng(Angle x, Angle min, Angle max){ - if(x.gt(max)) { - return max; - }else if(x.lt(min)) { - return min; - }else{ - return x; - } - } - - - - - - // // TODO: Test - // @Override - // public void setShooterAngle(ShooterState state, Angle angle) { - // state.shooterTargetAngle = angle; - // // Assume that the angle is always accurate, because I think we will use a shaft encoder - // // Assume that 0 degrees = forwards. Might need an offset here - - // Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT); - // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - // double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO; - // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - // m_angleMotor.setControl(posRequest); - // } - - - // TODO: Test - // @Override - // public void setShooterPitch(ShooterState state, Angle angle) { - // state.shooterTargetPitch = angle; - // // TODO: Test - // // This assumes that the 0 is paralell to the ground. Might need an offset here - - - // Angle boundedAngle = clampAng(angle, ShooterConstants.PITCH_LIMIT_UPPER, ShooterConstants.PITCH_LIMIT_LOWER); - // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - // double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO; - // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - // m_pitchMotor.setControl(posRequest); - // } - @Override public void setShooterVelocity(ShooterState state, AngularVelocity target) { state.motor1TargetVelocity = target;