diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 85587d2..9d1cf19 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,18 +19,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // Commands import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.MoveForTimeCommand; +import frc4388.robot.commands.MoveUntilSuply; import frc4388.robot.constants.Constants; +import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -39,6 +46,7 @@ import frc4388.utility.controller.DeadbandedXboxController; // Autos import frc4388.utility.controller.VirtualController; import frc4388.utility.controller.XboxController; +import frc4388.utility.structs.LEDPatterns; /** * This class is where the bulk of the robot should be declared. Since @@ -55,8 +63,8 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); - // public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); + private Boolean operatorManualMode = false; // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); // public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); @@ -81,6 +89,10 @@ public class RobotContainer { private SendableChooser autoChooser; private Command autoCommand; + private Command RobotShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED) + ); + public RobotContainer() { configureButtonBindings(); @@ -138,6 +150,8 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) // .onTrue(new RotTo45(m_robotSwerveDrive)); + new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) + .onTrue(RobotShoot); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); @@ -152,8 +166,11 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); - } + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + + } + +//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index a6c72ad..7c55826 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,13 +7,13 @@ 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 = 22; - public static final String GIT_SHA = "e2797ab77871b91d546e6e9793852cf94d8b712f"; - public static final String GIT_DATE = "2026-01-27 19:28:57 MST"; + public static final int GIT_REVISION = 23; + public static final String GIT_SHA = "8bda61b9837d7450a2fb4650bf02ffb1f4f06213"; + public static final String GIT_DATE = "2026-01-29 16:59:53 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-01-29 16:59:21 MST"; - public static final long BUILD_UNIX_TIME = 1769731161482L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2026-01-29 17:04:31 MST"; + public static final long BUILD_UNIX_TIME = 1769731471423L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index c050278..e095b06 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -30,7 +30,7 @@ public class LED extends SubsystemBase implements Queryable { private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN; - +//hello public void setMode(LEDPatterns pattern){ this.mode = pattern; } diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 1aac079..0693e0b 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -34,6 +34,7 @@ public class Intake extends SubsystemBase { switch (mode) { case Up: io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); + io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); break; case Down: io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 48600d5..0a1236d 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -33,7 +33,9 @@ public class IntakeConstants { // negative is left, positive is right public static final Angle ARM_LIMIT_LOWER = Degrees.of(-180); public static final Angle ARM_LIMIT_UPPER = Degrees.of(180); - public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0); + public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0); + public static final Slot0Configs ARM_PID = new Slot0Configs() .withKP(2.0) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 60ddaec..0c879f4 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,7 +1,5 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Rotation; - import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -9,7 +7,8 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.status.FaultReporter; +import frc4388.robot.subsystems.intake.IntakeConstants; +import frc4388.robot.subsystems.shooter.ShooterIO.ShooterState; public class Shooter extends SubsystemBase { ShooterIO io; @@ -17,6 +16,7 @@ public class Shooter extends SubsystemBase { Supplier m_swervePoseSupplier; + public Shooter( ShooterIO io, Supplier swervePoseSupplier @@ -33,12 +33,43 @@ 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, + } + + public void setMode(ShooterMode mode) { + switch (mode) { + case Active: + io.setMotor1Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); + io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); + io.setIndexerVelocity(state, ShooterConstants.INDEXER_ACTIVE_VELOCITY); + break; + case Resting: + io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY); + break; + case Inactive: + io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY); + break; + } + } + // 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; } + @Override public void periodic() { diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 1e80500..7e167cd 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -1,6 +1,7 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -9,6 +10,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import frc4388.utility.status.CanDevice; public class ShooterConstants { @@ -19,6 +21,15 @@ public class ShooterConstants { public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.; public static final double INDEXER_GEAR_RATIO = 1.; + public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + + + + public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) .withKP(0.0)