diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0b378cd..e51b19c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,6 +13,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.RobotBase; @@ -34,6 +35,7 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.shooter.Shooter; +import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -56,18 +58,23 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(Mode.REAL); + /*Limit Switch */ + public final DigitalInput m_armLimitSwitch = new DigitalInput(9); + /* Subsystems */ public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID); //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 Intake m_robotIntake = new Intake(m_robotMap.intakeIO, m_armLimitSwitch); public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + + // private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID); // public List subsystems = new ArrayList<>(); @@ -146,8 +153,8 @@ public class RobotContainer { private void configureButtonBindings() { - // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) @@ -209,25 +216,25 @@ public class RobotContainer { .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(), - FieldPositions.HUB_POSITION); + // m_robotSwerveDrive.driveFacingPosition( + // getDeadbandedDriverController().getLeft(), + // FieldPositions.HUB_POSITION); }, m_robotSwerveDrive) ) .onTrue(new InstantCommand(() -> { // When Right trigger is pressed, - m_robotIntake.setMode(IntakeMode.Extended); + m_robotShooter.setShooterReady(); })) .onFalse(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Retracted); - m_robotSwerveDrive.softStop(); - }, m_robotSwerveDrive)); + m_robotShooter.setShooterNotReady(); + })); // D-PAD fine alignment diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index abf86f9..4db6750 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,15 +5,15 @@ package frc4388.robot.constants; */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; + public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 40; - public static final String GIT_SHA = "983b95fdc704ef35b6f5e2dada2c6348f3c67190"; - public static final String GIT_DATE = "2026-02-10 19:42:47 MST"; + public static final int GIT_REVISION = 49; + public static final String GIT_SHA = "0425cdd0a1e794109348c7369dc21b377fca569b"; + public static final String GIT_DATE = "2026-02-12 14:36:26 MST"; public static final String GIT_BRANCH = "shoot-button"; - public static final String BUILD_DATE = "2026-02-11 12:41:33 MST"; - public static final long BUILD_UNIX_TIME = 1770838893524L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-02-14 11:48:31 MST"; + public static final long BUILD_UNIX_TIME = 1771094911048L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index af919f0..4483aa0 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -7,19 +7,24 @@ import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.intake.Intake.IntakeMode; public class Intake extends SubsystemBase { public IntakeIO io; IntakeStateAutoLogged state = new IntakeStateAutoLogged(); Supplier m_swervePoseSupplier; + public DigitalInput m_armLimitSwitch; public Intake( - IntakeIO io + IntakeIO io, + DigitalInput m_armLimitSwitch // Supplier swervePoseSupplier ) { this.io = io; + this.m_armLimitSwitch= m_armLimitSwitch; // this.m_swervePoseSupplier = swervePoseSupplier; } @@ -56,6 +61,7 @@ public class Intake extends SubsystemBase { @Override public void periodic() { // FaultReporter.register(this); // TODO Implement fault reporter + // System.out.println(m_armLimitSwitch.get()); Logger.processInputs("Intake", state); @@ -68,7 +74,12 @@ public class Intake extends SubsystemBase { io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); break; case Retracted: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); + if (!m_armLimitSwitch.get()){ + System.out.println("Triggered!"); + io.stopArm(); + } else { + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); + } io.setRollerVelocity(state, RotationsPerSecond.of(0)); break; } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 621f86b..9109d34 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -12,7 +12,7 @@ import frc4388.utility.status.CanDevice; public class IntakeConstants { // Motor conversions - public static final double ARM_MOTOR_GEAR_RATIO = 100; + public static final double ARM_MOTOR_GEAR_RATIO = 125; public static final double ROLLER_MOTOR_GEAR_RATIO = 3; @@ -68,7 +68,7 @@ public class IntakeConstants { public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimit(15) // TODO: tune??? .withStatorCurrentLimitEnable(true) ).withMotorOutput( new MotorOutputConfigs() diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 5e8f8b9..c5f5b81 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -33,6 +33,7 @@ public interface IntakeIO { // public default void setShooterAngle(ShooterState state, Angle angle) {} // public default void setShooterPitch(ShooterState state, Angle angle) {} public default void setArmAngle(IntakeState state, Angle angle) {} + public default void stopArm() {} public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {} public default void updateInputs(IntakeState state) {} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index c94b0d9..47f5b91 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -78,6 +78,10 @@ public class IntakeReal implements IntakeIO { // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); m_armMotor.setControl(armPosition.withPosition(motorAngle)); } + @Override + public void stopArm(){ + m_armMotor.set(0); + } @Override public void updateInputs(IntakeState state) { diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index ed71e49..da6e44b 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -97,54 +97,54 @@ public class Shooter extends SubsystemBase { // 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 shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; - boolean badShooterVelocity = shooterSpeed < ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); - boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended; + // boolean badShooterVelocity = shooterSpeed < ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); + // boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended; - int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (intakeBad ? 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: // Bad flywheel, no driver err - m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); - break; - case 0b011: // Bad flywheel, yes driver err - m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); - break; - case 0b100: // Bad intake, no driver err - m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT); - break; - case 0b101: // Bad intake, yes driver err - m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS); - break; - case 0b110: // Bad intake and shooter (intake is more important), no driver err - m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT); - break; - case 0b111: // Bad intake and shooter (intake is more important), yes driver err - m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS); - break; - } + // int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (intakeBad ? 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: // Bad flywheel, no driver err + // m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); + // break; + // case 0b011: // Bad flywheel, yes driver err + // m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); + // break; + // case 0b100: // Bad intake, no driver err + // m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT); + // break; + // case 0b101: // Bad intake, yes driver err + // m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS); + // break; + // case 0b110: // Bad intake and shooter (intake is more important), no driver err + // m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT); + // break; + // case 0b111: // Bad intake and shooter (intake is more important), yes driver err + // m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS); + // break; + // } - // We set the shooter mode to ready if there are no errors - mode = ( - bitmask == 0 ? - ShooterMode.Ready : - ShooterMode.NotReady - ); + // // We set the shooter mode to ready if there are no errors + // mode = ( + // bitmask == 0 ? + // ShooterMode.Ready : + // ShooterMode.NotReady + // ); - } + // } switch (mode) { case Shooting: @@ -162,4 +162,4 @@ public class Shooter extends SubsystemBase { } } -} + }} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 31ce6c6..1db52b5 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -24,8 +24,8 @@ public class ShooterConstants { // public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10); // public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); - public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30); - public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); + public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 0); + public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting 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);