From f191713fa461589ce6ff5ca7bc048d8b4391b458 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sun, 17 Mar 2024 12:25:37 -0400 Subject: [PATCH] manual controls on second controller for trap testing --- src/main/java/frc/robot/RobotContainer.java | 102 ++++++++---------- .../robot/subsystems/arm/ArmSubsystem.java | 67 +++--------- .../subsystems/shooter/ShooterSubsystem.java | 5 - 3 files changed, 62 insertions(+), 112 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6719a3a..64a49667 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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; @@ -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() { @@ -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()) @@ -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())) @@ -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))); @@ -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 @@ -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( @@ -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())); } /** diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 8f697b74..3a4bb33e 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -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; @@ -33,7 +32,8 @@ public enum ArmState { TRANSITION_SOURCE, PASS, DISABLED, - BACKUP_SHOT, MANUAL_WRIST + BACKUP_SHOT, + MANUAL_CONTROL } private final ArmIO m_io; @@ -41,14 +41,14 @@ public enum ArmState { 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; @@ -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; @@ -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; + }); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 5e90f074..2ade2e75 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -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); }