Skip to content

Commit

Permalink
manual controls on second controller for trap testing
Browse files Browse the repository at this point in the history
  • Loading branch information
@GearBoxFox
GearBoxFox committed Mar 17, 2024
1 parent 2790017 commit f191713
Show file tree
Hide file tree
Showing 3 changed files with 62 additions and 112 deletions.
102 changes: 45 additions & 57 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,6 @@
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.DriveConstants;
Expand Down Expand Up @@ -64,12 +63,15 @@ public class RobotContainer {
private final ClimberSubsystem m_climber;

// Controller
private final CommandXboxController controller = new CommandXboxController(0);
private final CommandXboxController m_driverController = new CommandXboxController(0);
private final CommandXboxController m_operatorController = new CommandXboxController(1);

private final LoggedDashboardNumber m_armIncrement = new LoggedDashboardNumber("Arm/Increment value", 1);
private final LoggedDashboardNumber m_leftPower = new LoggedDashboardNumber("Shooter/Left Power", 2250);
private final LoggedDashboardNumber m_rightPower = new LoggedDashboardNumber("Shooter/Right Power", 2250);

// Dashboard inputs
private final AutoFactory m_autonFactory;
private final LoggedDashboardNumber m_leftRPM = new LoggedDashboardNumber("Shooter/LeftRPM", 3600);
private final LoggedDashboardNumber m_rightRPM = new LoggedDashboardNumber("Shooter/RightRPM", 3600);

/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
Expand Down Expand Up @@ -161,32 +163,32 @@ public RobotContainer() {
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
Trigger intakeTrigger = controller.y().and(controller.x().negate())
.and(controller.a().negate()) // make sure we don't amp
.and(controller.b().negate())
.and(controller.leftTrigger().negate());
Trigger intakeTrigger = m_driverController.y().and(m_driverController.x().negate())
.and(m_driverController.a().negate()) // make sure we don't amp
.and(m_driverController.b().negate())
.and(m_driverController.leftTrigger().negate());

Trigger spinUpTrigger = controller.x().and(controller.y().negate())
.and(controller.a().negate()) // make sure we don't amp
.and(controller.b().negate());
Trigger spinUpTrigger = m_driverController.x().and(m_driverController.y().negate())
.and(m_driverController.a().negate()) // make sure we don't amp
.and(m_driverController.b().negate());

Trigger passSpinUpTrigger = controller.leftTrigger()
Trigger passSpinUpTrigger = m_driverController.leftTrigger()
.and(spinUpTrigger.negate())
.and(controller.y().negate());
.and(m_driverController.y().negate());

Trigger passTrigger = controller.leftTrigger()
Trigger passTrigger = m_driverController.leftTrigger()
.and(spinUpTrigger.negate())
.and(controller.y());
.and(m_driverController.y());

Trigger shootTrigger = controller.x().and(controller.y())
.and(controller.a().negate()) // make sure we don't amp
.and(controller.b().negate())
.and(controller.leftTrigger().negate());
Trigger shootTrigger = m_driverController.x().and(m_driverController.y())
.and(m_driverController.a().negate()) // make sure we don't amp
.and(m_driverController.b().negate())
.and(m_driverController.leftTrigger().negate());

Trigger ampLineupTrigger = controller.b().and(controller.a().negate())
Trigger ampLineupTrigger = m_driverController.b().and(m_driverController.a().negate())
.debounce(0.1, Debouncer.DebounceType.kBoth);

Trigger ampDepositeTrigger = controller.b().and(controller.a())
Trigger ampDepositeTrigger = m_driverController.b().and(m_driverController.a())
.and(spinUpTrigger.negate()) // make sure we don't amp while trying to do anything else
.and(shootTrigger.negate())
.and(intakeTrigger.negate())
Expand All @@ -195,26 +197,10 @@ private void configureButtonBindings() {
intakeTrigger.whileTrue(m_shooter.intakeCommand(0.75, 0.5, 0.1)
.alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE)));

// spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false)
// .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM))
// .alongWith(DriveCommands.alignmentDrive(
// m_driveSubsystem,
// () -> -controller.getLeftY(),
// () -> -controller.getLeftX(),
// () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER)
// )));
spinUpTrigger.whileTrue(new AimbotCommand(m_armSubsystem, m_driveSubsystem, m_shooter, controller.getHID(), false));

// shootTrigger.whileTrue(m_shooter.runShooterVelocity(true)
// .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM))
// .alongWith(DriveCommands.alignmentDrive(
// m_driveSubsystem,
// () -> -controller.getLeftY(),
// () -> -controller.getLeftX(),
// () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER)
// )));

shootTrigger.whileTrue(new AimbotCommand(m_armSubsystem, m_driveSubsystem, m_shooter, controller.getHID(), true));
spinUpTrigger.whileTrue(
new AimbotCommand(m_armSubsystem, m_driveSubsystem, m_shooter, m_driverController.getHID(), false));
shootTrigger.whileTrue(
new AimbotCommand(m_armSubsystem, m_driveSubsystem, m_shooter, m_driverController.getHID(), true));

ampLineupTrigger.whileTrue(m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP)
.finallyDo(() -> m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP).schedule()))
Expand All @@ -225,15 +211,6 @@ private void configureButtonBindings() {
m_shooter)
.alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)));


controller.leftBumper().whileTrue(
m_shooter.runShooterVelocity(false, m_leftRPM.get(), m_rightRPM.get())
.alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.MANUAL_WRIST)));

controller.rightBumper().whileTrue(
m_shooter.runShooterVelocity(true, m_leftRPM.get(), m_rightRPM.get())
.alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.MANUAL_WRIST)));

passSpinUpTrigger.whileTrue(
m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.PASS)
.alongWith(m_shooter.runShooterVelocity(false, 3500, 3500)));
Expand All @@ -242,10 +219,10 @@ private void configureButtonBindings() {
m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.PASS)
.alongWith(m_shooter.runShooterVelocity(true, 3500, 3500)));

controller.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP));
controller.pov(0).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE));
m_driverController.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP));
m_driverController.pov(0).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE));

controller.pov(90).whileTrue(m_shooter.intakeCommand(-0.75, -0.75, 0.0))
m_driverController.pov(90).whileTrue(m_shooter.intakeCommand(-0.75, -0.75, 0.0))
.whileFalse(m_shooter.intakeCommand(0.0, 0.0, 0.0));

// 96.240234375
Expand All @@ -255,10 +232,10 @@ private void configureButtonBindings() {
m_driveSubsystem.setDefaultCommand(
DriveCommands.joystickDrive(
m_driveSubsystem,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
controller
() -> -m_driverController.getLeftY(),
() -> -m_driverController.getLeftX(),
() -> -m_driverController.getRightX()));
m_driverController
.start()
.onTrue(
Commands.runOnce(
Expand All @@ -268,6 +245,17 @@ private void configureButtonBindings() {
Rotation2d.fromDegrees(180.0)))),
m_driveSubsystem)
.ignoringDisable(true));

m_operatorController.a().onTrue(m_armSubsystem.incrementArmManual(m_armIncrement.get()));
m_operatorController.b().onTrue(m_armSubsystem.incrementArmManual(-m_armIncrement.get()));

m_operatorController.x().onTrue(m_armSubsystem.incrementWristManual(m_armIncrement.get()));
m_operatorController.y().onTrue(m_armSubsystem.incrementWristManual(-m_armIncrement.get()));

m_operatorController.leftTrigger().whileTrue(
m_shooter.runShooterVelocity(false, m_leftPower.get(), m_rightPower.get()));
m_operatorController.rightTrigger().whileTrue(
m_shooter.runShooterVelocity(true, m_leftPower.get(), m_rightPower.get()));
}

/**
Expand Down
67 changes: 17 additions & 50 deletions src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.ArmConstants;
import frc.robot.Constants.ArmSetpoints;
import lib.logger.DataLogUtil;
import org.littletonrobotics.junction.Logger;
import lib.utils.AimbotUtils;
import lib.utils.AllianceFlipUtil;
Expand All @@ -33,22 +32,23 @@ public enum ArmState {
TRANSITION_SOURCE,
PASS,
DISABLED,
BACKUP_SHOT, MANUAL_WRIST
BACKUP_SHOT,
MANUAL_CONTROL
}

private final ArmIO m_io;
private final ArmIOInputsAutoLogged m_inputs;
private double m_desiredArmPoseDegs;
private double m_armVelocityMult = 0;
private double m_desiredWristPoseDegs;
private double m_wristGap;
private double m_wristVelocityMult = 0;
private boolean m_disabledBrakeMode = true;

private ArmState m_desiredState = ArmState.STOW;
private ArmState m_currentState = ArmState.DISABLED;

private final DataLogUtil.DataLogTable logUtil = DataLogUtil.getTable("Arm");
private double m_wristIncremental = 45.0;
private double m_armIncremental = 0.0;

private final ArmVisualizer m_setpointVisualizer;
private final ArmVisualizer m_poseVisualizer;
Expand Down Expand Up @@ -184,11 +184,9 @@ public void handleState() {
m_desiredWristPoseDegs = 50.0;
m_desiredArmPoseDegs = 0.0;
}
case MANUAL_WRIST -> {
m_desiredWristPoseDegs = ArmSetpoints.STATIC_SHOOTER.wristAngle();

m_desiredArmPoseDegs = ArmConstants.WRIST_ARM_GAP.getValue() - m_desiredWristPoseDegs;
m_desiredArmPoseDegs = Math.max(m_desiredArmPoseDegs, ArmSetpoints.STATIC_SHOOTER.armAngle());
case MANUAL_CONTROL -> {
m_desiredWristPoseDegs = m_wristIncremental;
m_desiredArmPoseDegs = m_armIncremental;
}
default -> {
m_armVelocityMult = 1.0;
Expand Down Expand Up @@ -231,48 +229,17 @@ public Command resetEncoderFactory() {
return runOnce(m_io::resetPosition).ignoringDisable(true);
}

public Command setArmPowerFactory(double power) {
return runEnd(() -> {
// let arm know it's in manual control
m_io.enableBrakeMode(true);
m_desiredArmPoseDegs = Double.NEGATIVE_INFINITY;

double finalPower = power;
// limiting code for arm
if (m_inputs.armPositionDegs > ArmConstants.ARM_UPPER_LIMIT.getValue()) {
finalPower = MathUtil.clamp(power, -1.0, 0.0);
} else if (m_inputs.armPositionDegs < ArmConstants.ARM_LOWER_LIMIT.getValue()) {
finalPower = MathUtil.clamp(power, 0.0, 1.0);
}
m_io.setArmVoltage(finalPower * 12.0);

// check to see if wrist is too close, if it is back drive it
if (m_inputs.armPositionDegs + m_inputs.wristPositionDegs < ArmConstants.WRIST_ARM_GAP.getValue()) {
m_desiredWristPoseDegs = Double.NEGATIVE_INFINITY;
m_io.setWristVoltage(Math.abs(finalPower));
}
},
() -> m_io.setArmVoltage(0.0));
public Command incrementArmManual(double increment) {
return run(() -> {
m_desiredState = ArmState.MANUAL_CONTROL;
m_armIncremental += increment;
});
}

public Command setWristPowerFactory(double power) {
return runEnd(() -> {
// let wrist know it's in manual control mode
m_io.enableBrakeMode(true);
m_desiredWristPoseDegs = Double.NEGATIVE_INFINITY;

// limiting code for wrist
final double outPower;
if (m_inputs.wristPositionDegs > ArmConstants.WRIST_UPPER_LIMIT.getValue()) {
outPower = MathUtil.clamp(power, -1.0, 0.0);
} else if (m_inputs.wristPositionDegs < ArmConstants.WRIST_LOWER_LIMIT.getValue()
|| m_inputs.wristPositionDegs + m_inputs.armPositionDegs < ArmConstants.WRIST_ARM_GAP.getValue()) {
outPower = MathUtil.clamp(power, 0.0, 1.0);
} else {
outPower = power;
}
m_io.setWristVoltage(outPower * 12.0);
},
() -> m_io.setWristVoltage(0.0));
public Command incrementWristManual(double increment) {
return run(() -> {
m_desiredState = ArmState.MANUAL_CONTROL;
m_wristIncremental += increment;
});
}
}
5 changes: 0 additions & 5 deletions src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,11 +30,6 @@ public class ShooterSubsystem extends SubsystemBase {
private double m_leftSpeedSetpoint = 3800.0;
private double m_rightSpeedSetpoint = 3800.0;

private final GosDoubleProperty m_leftPower = new
GosDoubleProperty(false, "Shooter/Left RPM", 3600);
private final GosDoubleProperty m_rightPower = new
GosDoubleProperty(false, "Shooter/Right RPM", 3600);

public ShooterSubsystem(ShooterIO io) {
this(io, Pose2d::new);
}
Expand Down

0 comments on commit f191713

Please sign in to comment.