diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index b70f5cd..271f901 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -82,7 +82,6 @@ public class RobotContainer implements Sendable { public final GenericHID driverController = new GenericHID(Driver.port); - public final XboxController driverRumble = new XboxController(Driver.port); //For rumbling the controller public final GenericHID manipulatorController = new GenericHID(Manipulator.port); public final Limelight limelight = new Limelight(); @@ -146,7 +145,7 @@ private void setBindingsManipulator() { .whileTrue(new RunConveyor(intake)); //could be toggle mode instead // .whileTrue(new OuttakeFeeder(outtake)); //could be toggle mode instead new JoystickButton(manipulatorController, Manipulator.INTAKE_BUTTON) - .whileTrue(new IntakeBalls(intake)); + .whileTrue(new IntakeBalls(intake, manipulatorController)); new JoystickButton(manipulatorController, Manipulator.OUTTAKE_BUTTON) .whileTrue(new ShootBalls(outtake)); axisTrigger(manipulatorController, Manipulator.SMART_SHOOT_AXIS, OI.JOY_THRESH) @@ -172,7 +171,7 @@ private void setDefaultCommands() { () -> SmartDashboard.getBoolean("Baby Mode", Config.CONFIG.isBabyMode()) )); - intake.setDefaultCommand(new IntakeBalls(intake)); + intake.setDefaultCommand(new IntakeBalls(intake, manipulatorController)); } //#endregion //#region getAutoCommand diff --git a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java index 855c509..06145a6 100644 --- a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java +++ b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java @@ -8,15 +8,21 @@ import org.carlmontrobotics.subsystems.Intake; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class IntakeBalls extends Command { Intake intake; + GenericHID manipulatorRumble; /** Creates a new IntakeBalls. */ - public IntakeBalls(Intake intake) { + public IntakeBalls(Intake intake, GenericHID manipulatorRumble) { this.intake = intake; + this.manipulatorRumble = manipulatorRumble; addRequirements(intake); // Use addRequirements() here to declare subsystem dependencies. } @@ -29,12 +35,22 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if (checkIfJammed()) { + SmartDashboard.putBoolean("IntakeJammed", true); + manipulatorRumble.setRumble(RumbleType.kRightRumble, 0.5); + } + else { + SmartDashboard.putBoolean("IntakeJammed", true); + manipulatorRumble.setRumble(RumbleType.kRightRumble, 0); + } + } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { intake.stopIntake(); + manipulatorRumble.setRumble(RumbleType.kRightRumble, 0); } // Returns true when the command should end. @@ -42,4 +58,8 @@ public void end(boolean interrupted) { public boolean isFinished() { return false; } + + private boolean checkIfJammed() { + return intake.getIntakeVelocity() < 20; + } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Intake.java b/src/main/java/org/carlmontrobotics/subsystems/Intake.java index 85fedaf..27d5802 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Intake.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Intake.java @@ -62,4 +62,28 @@ public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("rpm brr", intakeMotor.getEncoder().getVelocity()); } + + /** + * + * @return Number the RPM of the motor + */ + public double getIntakeVelocity() { + return intakeMotor.getEncoder().getVelocity(); + } + + /** + * + * @return The motor controller's output current in Amps. + */ + public double getIntakeCurrent() { + return intakeMotor.getOutputCurrent(); + } + /** + * + * @return The motor controller's applied output duty cycle. + + */ + public double getIntakeAppliedOutput() { + return intakeMotor.getAppliedOutput(); + } }