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] 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();