From ddbae688512e74b8f06d5b0e3d0a1d156e9c9f82 Mon Sep 17 00:00:00 2001 From: McGrathMaggie <78870mm@gmail.com> Date: Tue, 25 Feb 2025 16:41:28 -0700 Subject: [PATCH 1/5] Created Driver Controls Can switch between programming controls and driver controls now --- .../java/frc4388/robot/RobotContainer.java | 256 ++++++++++++------ 1 file changed, 166 insertions(+), 90 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 00e4f50..d15d3ed 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -357,118 +357,194 @@ public class RobotContainer { */ private void configureButtonBindings() { + boolean driverPreferenceControls = false; + // ? /* Driver Buttons */ - DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - // ! /* Speed */ - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + if (!driverPreferenceControls) { + + DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); + // ! /* Speed */ + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - + new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + - new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) - .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp())); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) - .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) + .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown())); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) - .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) + .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp())); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) - .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) + .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); + } else { + + } // ? /* Operator Buttons */ - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); + if (!driverPreferenceControls) { - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)); - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator)); - + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); - new JoystickButton(getButtonBox(), ButtonBox.Five) - .onTrue(AprilLidarAlignL4Left); - - new JoystickButton(getButtonBox(), ButtonBox.One) - .onTrue(AprilLidarAlignL4Right); - - new JoystickButton(getButtonBox(), ButtonBox.Six) - .onTrue(AprilLidarAlignL3Left); - - new JoystickButton(getButtonBox(), ButtonBox.Two) - .onTrue(AprilLidarAlignL3Right); - - new JoystickButton(getButtonBox(), ButtonBox.Seven) - .onTrue(AprilLidarAlignL2Left); - - new JoystickButton(getButtonBox(), ButtonBox.Three) - .onTrue(AprilLidarAlignL2Right); - - // Lower coral removal - new JoystickButton(getButtonBox(), ButtonBox.Eight) - .onTrue(leftAlgaeRemove); - - // Upper coral removal - new JoystickButton(getButtonBox(), ButtonBox.Four) - .onTrue(rightAlgaeRemove); - - - // Cancel button - new JoystickButton(getButtonBox(), ButtonBox.White) - .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator)); - - // Score prep - // Prime 4 - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator)); - - // Prime 3 - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator)); - - - - //Trims - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) - .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp())); - - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) - .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown())); - - - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90) - .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp())); - - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270) - .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown())); + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)); + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator)); + + + new JoystickButton(getButtonBox(), ButtonBox.Five) + .onTrue(AprilLidarAlignL4Left); + + new JoystickButton(getButtonBox(), ButtonBox.One) + .onTrue(AprilLidarAlignL4Right); + + new JoystickButton(getButtonBox(), ButtonBox.Six) + .onTrue(AprilLidarAlignL3Left); + + new JoystickButton(getButtonBox(), ButtonBox.Two) + .onTrue(AprilLidarAlignL3Right); + + new JoystickButton(getButtonBox(), ButtonBox.Seven) + .onTrue(AprilLidarAlignL2Left); + + new JoystickButton(getButtonBox(), ButtonBox.Three) + .onTrue(AprilLidarAlignL2Right); + + + // Lower algae removal + new JoystickButton(getButtonBox(), ButtonBox.Eight) + .onTrue(leftAlgaeRemove); + + // Upper algae removal + new JoystickButton(getButtonBox(), ButtonBox.Four) + .onTrue(rightAlgaeRemove); + + + // Cancel button + new JoystickButton(getButtonBox(), ButtonBox.White) + .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator)); + + // Score prep + // Prime 4 + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator)); + + // Prime 3 + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator)); + + + + //Trims + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) + .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp())); + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) + .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown())); + + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90) + .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp())); + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270) + .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown())); + + } else { + + // States: ready/waiting + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator)); + + //Controller Coral Scoring + new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) + .onTrue(AprilLidarAlignL4Left); + + new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) + .onTrue(AprilLidarAlignL4Right); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(AprilLidarAlignL3Left); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(AprilLidarAlignL3Right); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .onTrue(AprilLidarAlignL2Left); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(AprilLidarAlignL2Right); + + //Controller Lower Algae Removal + + + //Controller Upper Algae Removal + + //Button Box Coral Scoring + new JoystickButton(getButtonBox(), ButtonBox.Five) + .onTrue(AprilLidarAlignL4Left); + + new JoystickButton(getButtonBox(), ButtonBox.One) + .onTrue(AprilLidarAlignL4Right); + + new JoystickButton(getButtonBox(), ButtonBox.Six) + .onTrue(AprilLidarAlignL3Left); + + new JoystickButton(getButtonBox(), ButtonBox.Two) + .onTrue(AprilLidarAlignL3Right); + + new JoystickButton(getButtonBox(), ButtonBox.Seven) + .onTrue(AprilLidarAlignL2Left); + + new JoystickButton(getButtonBox(), ButtonBox.Three) + .onTrue(AprilLidarAlignL2Right); + + // Button Box Lower algae removal + new JoystickButton(getButtonBox(), ButtonBox.Eight) + .onTrue(leftAlgaeRemove); + + // Button Box Upper algae removal + new JoystickButton(getButtonBox(), ButtonBox.Four) + .onTrue(rightAlgaeRemove); + + // Cancel button + new JoystickButton(getButtonBox(), ButtonBox.White) + .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator)); + + } + // ? /* Programer Buttons (Controller 3)*/ // * /* Auto Recording */ From b341acaa3dd92ac996db3c4b531605658d72d40c Mon Sep 17 00:00:00 2001 From: McGrathMaggie <78870mm@gmail.com> Date: Tue, 25 Feb 2025 19:47:57 -0700 Subject: [PATCH 2/5] Preferred operator controls Added preferred operator controls Added manual controls Renamed misspelled items --- src/main/java/frc4388/robot/Constants.java | 18 ++--- .../java/frc4388/robot/RobotContainer.java | 19 ++++- src/main/java/frc4388/robot/RobotMap.java | 2 +- .../robot/commands/waitElevatorRefrence.java | 2 +- .../commands/waitEndefectorRefrence.java | 2 +- .../frc4388/robot/subsystems/Elevator.java | 76 ++++++++++--------- 6 files changed, 69 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6e73085..cc7a8d3 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -237,7 +237,7 @@ public final class Constants { public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0); - public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR, 1, 0); + public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0); public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole public static final int LIDAR_DIO_CHANNEL = 7; @@ -430,21 +430,21 @@ public final class Constants { public static final double ENDEFECTOR_DRIVE_SLOW = -0.08; //Max for endefector = 60% - public static final double L2_SCORE_ENDEFECTOR = -19; + public static final double L2_SCORE_ENDEFFECTOR = -19; - public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; - public static final double DEALGAE_L2_EENDEFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR; - public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; - public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; - public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value - public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; + public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; + public static final double DEALGAE_L2_ENDEFFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR; + public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; + public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; + public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value + public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() .withKP(1) .withKI(0) .withKD(0); - public static final Slot0Configs ENDEFECTOR_PID = new Slot0Configs() + public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs() .withKP(1) .withKI(0) .withKD(0); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d15d3ed..82bcfbd 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.utility.controller.XboxController; @@ -83,7 +84,7 @@ public class RobotContainer { // private final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); - public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endefectorLimitSwitch); + public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -452,7 +453,7 @@ public class RobotContainer { // Cancel button new JoystickButton(getButtonBox(), ButtonBox.White) - .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator)); + .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator)); // Score prep // Prime 4 @@ -508,9 +509,12 @@ public class RobotContainer { .onTrue(AprilLidarAlignL2Right); //Controller Lower Algae Removal - + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) + .onTrue(leftAlgaeRemove); //Controller Upper Algae Removal + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) + .onTrue(rightAlgaeRemove); //Button Box Coral Scoring new JoystickButton(getButtonBox(), ButtonBox.Five) @@ -541,7 +545,14 @@ public class RobotContainer { // Cancel button new JoystickButton(getButtonBox(), ButtonBox.White) - .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator)); + .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator)); + + //Manual Controls + m_robotElevator.setDefaultCommand(new RunCommand(() -> { + m_robotElevator.manualEndeffectorVel(getDeadbandedOperatorController().getLeftY()); + m_robotElevator.manualElevatorVel(getDeadbandedOperatorController().getRightY()); + }, m_robotElevator) + .withName("Default Manual Controls")); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index d996288..0313a70 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -57,7 +57,7 @@ public class RobotMap { public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); - public final DigitalInput endefectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); + public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); void configureDriveMotorControllers() {} diff --git a/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java b/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java index d6d803d..f3bd0fc 100644 --- a/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java +++ b/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java @@ -31,6 +31,6 @@ public class waitElevatorRefrence extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return elevator.elevatorAtRefrence(); + return elevator.elevatorAtReference(); } } diff --git a/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java b/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java index 4a1a83e..19fe2a8 100644 --- a/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java +++ b/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java @@ -31,6 +31,6 @@ public class waitEndefectorRefrence extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return elevator.endefectorAtRefrence(); + return elevator.endeffectorAtReference(); } } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 0601c39..1324c42 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -24,20 +24,20 @@ import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; public class Elevator extends SubsystemBase { /** Creates a new Elevator. */ private TalonFX elevatorMotor; - private TalonFX endefectorMotor; + private TalonFX endeffectorMotor; private long wait = 0; private long maxWait = 1000; private double elevatorRefrence = 0; - private double endefectorRefrence = 0; + private double endeffectorRefrence = 0; private DigitalInput basinBeamBreak; - private DigitalInput endefectorLimitSwitch; + private DigitalInput endeffectorLimitSwitch; public enum CoordinationState { Waiting, // for coral into the though - WatingBeamTriped, //once the beam break trips + WatingBeamTripped, //once the beam break trips Ready, // Has coral in endefector Hovering, // Has coral in endefector L2Score, @@ -53,18 +53,18 @@ public class Elevator extends SubsystemBase { private CoordinationState currentState; - public Elevator(TalonFX elevatorTalonFX, TalonFX endefectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endefectorLimitSwitch) { + public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch) { elevatorMotor = elevatorTalonFX; - endefectorMotor = endefectorTalonFX; + endeffectorMotor = endeffectorTalonFX; this.basinBeamBreak = basinLimitSwitch; - this.endefectorLimitSwitch = endefectorLimitSwitch; + this.endeffectorLimitSwitch = endeffectorLimitSwitch; elevatorMotor.setNeutralMode(NeutralModeValue.Brake); - endefectorMotor.setNeutralMode(NeutralModeValue.Brake); + endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); - endefectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFECTOR_PID); + endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID); currentState = CoordinationState.Ready; } @@ -72,7 +72,7 @@ public class Elevator extends SubsystemBase { private void PIDPosition(TalonFX motor, double position) { if (motor == elevatorMotor) elevatorRefrence = position; - else endefectorRefrence = position; + else endeffectorRefrence = position; var request = new PositionDutyCycle(position); motor.setControl(request); @@ -82,8 +82,8 @@ public class Elevator extends SubsystemBase { elevatorMotor.set(0); } - public void endefectorStop() { - endefectorMotor.set(0); + public void endeffectorStop() { + endeffectorMotor.set(0); } public void transitionState(CoordinationState state) { @@ -92,79 +92,79 @@ public class Elevator extends SubsystemBase { case Waiting: { wait = System.currentTimeMillis() + maxWait; PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); break; } - case WatingBeamTriped: { + case WatingBeamTripped: { PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); break; } case Ready: { PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); break; } case Hovering: { PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); break; } case L2Score: { PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endefectorMotor, ElevatorConstants.L2_SCORE_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); break; } case PrimedFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR); break; } case ScoringFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); break; } case PrimedThree: { PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); break; } case ScoringThree: { PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); break; } case BallRemoverL2Primed: { PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR); break; } case BallRemoverL2Go: { PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); break; } case BallRemoverL3Primed: { PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR); break; } case BallRemoverL3Go: { PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); break; } @@ -175,7 +175,7 @@ public class Elevator extends SubsystemBase { } - public boolean elevatorAtRefrence() { + public boolean elevatorAtReference() { // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble(); double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.5; @@ -187,11 +187,11 @@ public class Elevator extends SubsystemBase { return atPos; } - public boolean endefectorAtRefrence() { + public boolean endeffectorAtReference() { // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); - double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble(); + double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble(); - return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2; + return Math.abs(endeffectorRefrence - endeffectorPosition) <= 0.2; } // public void driveElevatorStick(Translation2d stick) { // if (stick.getNorm() > 0.05) { @@ -210,12 +210,20 @@ public class Elevator extends SubsystemBase { // } private void periodicReady() { - if (elevatorAtRefrence()) + if (elevatorAtReference()) transitionState(CoordinationState.Hovering); } private void periodicScoring() { - if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting); + if (!endeffectorLimitSwitch.get()) transitionState(CoordinationState.Waiting); + } + + public void manualElevatorVel(double velocity) { + elevatorMotor.set(velocity); + } + + public void manualEndeffectorVel(double velocity) { + endeffectorMotor.set(velocity); } @Override @@ -223,12 +231,12 @@ public class Elevator extends SubsystemBase { // This method will be called once per scheduler run SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); - SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0); + SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); SmartDashboard.putString("State", currentState.toString()); if (currentState == CoordinationState.Waiting) { // periodicWaiting(); - } else if (currentState == CoordinationState.WatingBeamTriped) { + } else if (currentState == CoordinationState.WatingBeamTripped) { // periodicWaitingTripped(); } else if (currentState == CoordinationState.Ready) { periodicReady(); From d59e2f06da838e35c4d5fe211fa31b2523b63520 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 26 Feb 2025 16:53:15 -0700 Subject: [PATCH 3/5] fix: manual elevator methods needs to check if the joystick is zero. --- .../frc4388/robot/subsystems/Elevator.java | 21 ++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 1324c42..eb0366d 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -32,6 +32,9 @@ public class Elevator extends SubsystemBase { private double elevatorRefrence = 0; private double endeffectorRefrence = 0; + private boolean elevatorManualStop = true; + private boolean endefectorManualStop = true; + private DigitalInput basinBeamBreak; private DigitalInput endeffectorLimitSwitch; @@ -219,11 +222,23 @@ public class Elevator extends SubsystemBase { } public void manualElevatorVel(double velocity) { - elevatorMotor.set(velocity); + if (Math.abs(velocity) > 0.1) { + elevatorMotor.set(velocity); + } + if (!elevatorManualStop) { + elevatorManualStop = true; + elevatorMotor.set(0); + } } public void manualEndeffectorVel(double velocity) { - endeffectorMotor.set(velocity); + if (Math.abs(velocity) > 0.1) { + endeffectorMotor.set(velocity); + } + if (!endefectorManualStop) { + endefectorManualStop = true; + endeffectorMotor.set(0); + } } @Override @@ -235,7 +250,7 @@ public class Elevator extends SubsystemBase { SmartDashboard.putString("State", currentState.toString()); if (currentState == CoordinationState.Waiting) { - // periodicWaiting(); + periodicWaiting(); } else if (currentState == CoordinationState.WatingBeamTripped) { // periodicWaitingTripped(); } else if (currentState == CoordinationState.Ready) { From e7c59b7d58b2ca4177e1dad3b53e11cbcb8d7fc5 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 26 Feb 2025 16:55:20 -0700 Subject: [PATCH 4/5] up thresholds --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 82bcfbd..02c797c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -490,10 +490,10 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator)); //Controller Coral Scoring - new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) + new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8) .onTrue(AprilLidarAlignL4Left); - new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) + new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8) .onTrue(AprilLidarAlignL4Right); new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) From a24b7ea0e7dce6bdda20e19e2c5d49cf74e2e388 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 26 Feb 2025 17:14:17 -0700 Subject: [PATCH 5/5] fix faulty merge conflict managements --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Elevator.java | 7 +++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4ce4eb5..caec0f0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -88,7 +88,7 @@ public class RobotContainer { // private final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); - public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -494,7 +494,7 @@ public class RobotContainer { new JoystickButton(getButtonBox(), ButtonBox.White) .onTrue(new InstantCommand(() -> { m_robotElevator.elevatorStop(); - m_robotElevator.endefectorStop(); + m_robotElevator.endeffectorStop(); m_robotSwerveDrive.endSlowPeriod(); }, m_robotElevator)); diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index f58dbee..99ce643 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -193,12 +193,11 @@ public class Elevator extends SubsystemBase { public boolean endeffectorAtReference() { // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble(); - double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble(); - double diffrence = endefectorRefrence - endefectorPosition; + double diffrence = endeffectorRefrence - endeffectorPosition; boolean headedUp = diffrence < 0; - boolean forwardLimit = endefectorMotor.getForwardLimit().asSupplier().get().value == 0; - boolean reverseLimit = endefectorMotor.getReverseLimit().asSupplier().get().value == 0; + boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0; + boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0; return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); }