From 2d9ed527bebbf79d7643452fe4344ad32b3fab49 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Sat, 7 Feb 2026 14:51:05 -0700 Subject: [PATCH] instant commands for testing --- .../java/frc4388/robot/RobotContainer.java | 40 +++++++++++++------ src/main/java/frc4388/robot/RobotMap.java | 26 ++++++------ .../robot/subsystems/intake/Intake.java | 10 ++--- .../subsystems/intake/IntakeConstants.java | 8 ++-- .../robot/subsystems/shooter/Shooter.java | 12 +++--- .../subsystems/shooter/ShooterConstants.java | 6 ++- 6 files changed, 61 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e317919..70c894c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -33,6 +33,8 @@ import frc4388.robot.constants.Constants.SimConstants.Mode; import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -58,9 +60,10 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); //Testing of Colors - 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); private Boolean operatorManualMode = false; // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -86,13 +89,21 @@ public class RobotContainer { private SendableChooser autoChooser; private Command autoCommand; + // private Command RobotShoot = new SequentialCommandGroup( + // new InstantCommand(() -> System.out.println(m_robotLED.getMode())), + // new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED), + // new InstantCommand(() -> System.out.println(m_robotLED.getMode())), + // new WaitCommand(5), + // new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED), + // new InstantCommand(() -> System.out.println(m_robotLED.getMode())) + // ); + private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> System.out.println(m_robotLED.getMode())), - new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED), - new InstantCommand(() -> System.out.println(m_robotLED.getMode())), - new WaitCommand(5), - new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED), - new InstantCommand(() -> System.out.println(m_robotLED.getMode())) + new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter) + ); + + private Command RobotIntake = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake) ); public RobotContainer() { @@ -144,7 +155,12 @@ public class RobotContainer { // .onTrue(new RotTo45(m_robotSwerveDrive)); new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(RobotShoot); + .onTrue(RobotShoot) + .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter)); + + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) + .onTrue(RobotIntake) + .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter)); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); @@ -157,11 +173,11 @@ public class RobotContainer { - new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); // IF the driver is holding the aim button, aim the robot towards the hub new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 10bc3bc..cabf7b1 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -61,9 +61,9 @@ public class RobotMap { /* Swreve Drive Subsystem */ public final SwerveIO swerveDrivetrain; - // /* Elevator Subsystem */ - // public final ShooterIO shooterIO; - // public final IntakeIO intakeIO; + // /* Shooter and Intake Subsystem */ + public final ShooterIO shooterIO; + public final IntakeIO intakeIO; public RobotMap(SimConstants.Mode mode) { switch (mode) { @@ -91,23 +91,23 @@ public class RobotMap { swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); - // Configure Shooter + // Configure Shooter 22,23,24 TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id); TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id); TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); - //Configure Intake - // TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); - // TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); + //Configure Intake 20,21 + TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); + TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - // shooterIO = new ShooterReal(shooter1, shooter2, indexer); + shooterIO = new ShooterReal(shooter1, shooter2, indexer); - // intakeIO = new IntakeReal(arm, roller); + intakeIO = new IntakeReal(arm, roller); // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); @@ -116,8 +116,8 @@ public class RobotMap { FaultTalonFX.addDevice(shooter1, "Shooter1"); FaultTalonFX.addDevice(shooter2, "Shooter2"); FaultTalonFX.addDevice(indexer, "Indexer"); - // FaultTalonFX.addDevice(arm, "Arm"); - // FaultTalonFX.addDevice(roller, "Roller"); + FaultTalonFX.addDevice(arm, "Arm"); + FaultTalonFX.addDevice(roller, "Roller"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); @@ -139,8 +139,8 @@ public class RobotMap { // leftCamera = new VisionIO() {}; // rightCamera = new VisionIO() {}; swerveDrivetrain = new SwerveIO() {}; - // intakeIO = new IntakeIO() {}; - // shooterIO = new ShooterIO() {}; + intakeIO = new IntakeIO() {}; + shooterIO = new ShooterIO() {}; break; } } diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 0693e0b..c2ecd18 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -18,11 +18,11 @@ public class Intake extends SubsystemBase { Supplier m_swervePoseSupplier; public Intake( - IntakeIO io, - Supplier swervePoseSupplier + IntakeIO io + // Supplier swervePoseSupplier ) { this.io = io; - this.m_swervePoseSupplier = swervePoseSupplier; + // this.m_swervePoseSupplier = swervePoseSupplier; } public enum IntakeMode { @@ -33,11 +33,11 @@ public class Intake extends SubsystemBase { public void setMode(IntakeMode mode) { switch (mode) { case Up: - io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); + // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); break; case Down: - io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); + // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); break; } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 0a1236d..197a789 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -23,7 +23,6 @@ public class IntakeConstants { //IDs - public static final CanDevice ARM_ID = new CanDevice("SHOOTER 1", 20); public static final CanDevice ROLLER_ID = new CanDevice("SHOOTER 2", 21); @@ -31,8 +30,11 @@ public class IntakeConstants { // 0 is the forward angle on the robot. // 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); + + //when testing the negative output of 10% made the robot put the intake up + + public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); + public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0); public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 0c879f4..2809bd2 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -14,15 +14,15 @@ public class Shooter extends SubsystemBase { ShooterIO io; ShooterStateAutoLogged state = new ShooterStateAutoLogged(); - Supplier m_swervePoseSupplier; + // Supplier m_swervePoseSupplier; public Shooter( - ShooterIO io, - Supplier swervePoseSupplier + ShooterIO io + // Supplier swervePoseSupplier ) { this.io = io; - this.m_swervePoseSupplier = swervePoseSupplier; + // this.m_swervePoseSupplier = swervePoseSupplier; } public enum FieldZone { @@ -80,8 +80,8 @@ public class Shooter extends SubsystemBase { Logger.processInputs("Shooter", state); - Pose2d pose = m_swervePoseSupplier.get(); - Angle robotRot = pose.getRotation().getMeasure(); + // Pose2d pose = m_swervePoseSupplier.get(); + // Angle robotRot = pose.getRotation().getMeasure(); io.updateInputs(state); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 7e167cd..64bab3e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -22,9 +22,10 @@ public class ShooterConstants { 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_ACTIVE_VELOCITY = RotationsPerSecond.of(4.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_ACTIVE_VELOCITY = RotationsPerSecond.of(4.0); public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); @@ -59,6 +60,7 @@ public class ShooterConstants { // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means // ); + public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22); public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23); public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24);