Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
fix(intake): Rumble on note.
Browse files Browse the repository at this point in the history
haydenheroux committed Apr 10, 2024
1 parent 953ec1f commit 847a180
Showing 3 changed files with 27 additions and 6 deletions.
21 changes: 21 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
@@ -1,8 +1,12 @@
package frc.robot;

import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.lib.Telemetry;
import frc.robot.arm.Arm;
import frc.robot.auto.Auto;
@@ -29,6 +33,8 @@ public class RobotContainer {

private final CommandXboxController driverController, operatorController;

private final XboxController rumbleController;

/** Creates a new instance of the robot container. */
private RobotContainer() {
arm = Arm.getInstance();
@@ -41,6 +47,7 @@ private RobotContainer() {

driverController = new CommandXboxController(0);
operatorController = new CommandXboxController(1);
rumbleController = new XboxController(1);

initializeTelemetry();
configureDefaultCommands();
@@ -88,6 +95,20 @@ private void configureBindings() {
operatorController.a().onTrue(superstructure.amp());
operatorController.x().onTrue(superstructure.stow());
operatorController.y().onTrue(superstructure.climb());

new Trigger(intake::rollerNote).onTrue(rumbleOn()).onFalse(rumbleOff());
}

public void rumble(double value) {
rumbleController.setRumble(RumbleType.kBothRumble, value);
}

public Command rumbleOn() {
return Commands.runOnce(() -> rumble(1));
}

public Command rumbleOff() {
return Commands.runOnce(() -> rumble(0));
}

/**
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/intake/Intake.java
Original file line number Diff line number Diff line change
@@ -64,18 +64,18 @@ public void addToShuffleboard(ShuffleboardTab tab) {
VelocityControllerIO.addToShuffleboard(tab, "Front Roller", frontRollerValues);
VelocityControllerIO.addToShuffleboard(tab, "Back Roller", backRollerValues);

tab.addBoolean("Roller Stuck?", this::rollerStuck);
tab.addBoolean("Roller Note?", this::rollerNote);
}

public boolean frontRollerStuck() {
return frontRollerValues.motorAmps > FrontRollerConstants.STUCK_AMPS;
return frontRollerValues.motorAmps > FrontRollerConstants.NOTE_AMPS;
}

public boolean backRollerStuck() {
return backRollerValues.motorAmps > BackRollerConstants.STUCK_AMPS;
return backRollerValues.motorAmps > BackRollerConstants.NOTE_AMPS;
}

public boolean rollerStuck() {
public boolean rollerNote() {
return frontRollerStuck() || backRollerStuck();
}

4 changes: 2 additions & 2 deletions src/main/java/frc/robot/intake/IntakeConstants.java
Original file line number Diff line number Diff line change
@@ -37,7 +37,7 @@ public static class FrontRollerConstants {
/** Maximum speed of the roller in rotations per second. */
public static final double MAXIMUM_SPEED = 67;

public static final double STUCK_AMPS = 100;
public static final double NOTE_AMPS = 20;
}

/** Constants for the back roller. */
@@ -71,6 +71,6 @@ public static class BackRollerConstants {
/** Maximum speed of the roller in rotations per second. */
public static final double MAXIMUM_SPEED = 67;

public static final double STUCK_AMPS = 80;
public static final double NOTE_AMPS = 20;
}
}

0 comments on commit 847a180

Please sign in to comment.