From 2b376d741c83a007ff9116e23e3c1346ba7d3881 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 27 Jan 2025 17:29:25 -0700 Subject: [PATCH 1/2] Intergrate endefector into elevator for cordination --- src/main/java/frc4388/robot/Constants.java | 33 ++++---- .../java/frc4388/robot/RobotContainer.java | 51 ++++-------- src/main/java/frc4388/robot/RobotMap.java | 8 +- .../frc4388/robot/subsystems/Elevator.java | 83 ++++++++++++++----- .../frc4388/robot/subsystems/Endeffector.java | 51 ------------ 5 files changed, 101 insertions(+), 125 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/Endeffector.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5be92c8..60c60b0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -390,34 +390,33 @@ public final class Constants { } public static final class ElevatorConstants { + public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15); public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16); - - public static final double GEAR_RATIO = 36.0; - public static final double LEVEL_1 = 0 * GEAR_RATIO; - public static final double LEVEL_2 = 5 * GEAR_RATIO; - public static final double ELEVATOR_MAX_HEIGHT = 5 * GEAR_RATIO; - public static final double ELEVATOR_SPEED_UP = 1 * GEAR_RATIO; - public static final double ELEVATOR_SPEED_DOWN = -1 * GEAR_RATIO; + public static final int BASIN_LIMIT_SWITCH = 0; // TODO: FIND + public static final int ENDEFFECTOR_LIMIT_SWITCH = 1; // TODO: FIND + + public static final double GEAR_RATIO_ELEVATOR = 36.0; + + public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; + public static final double WAITING_POSITION_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find 4-6 off the ground + public static final double MAX_POSITION_ELEVATOR = 20 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position + + public static final double GEAR_RATIO_ENDEFECTOR = 100.0; + + public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; + public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; + public static final double SCORING_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value + public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() .withKP(1) .withKI(0) .withKD(0); - } - - public static class EndeffectorConstants { - public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15); - public static final double GEAR_RATIO = 100.0; - public static final double TOP = 0.25 * GEAR_RATIO; - public static final double MIDDLE = 0.0 * GEAR_RATIO; - public static final double BOTTOM = -0.25 * GEAR_RATIO; - public static final Slot0Configs ENDEFECTOR_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 7735d99..c4fd224 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -49,7 +49,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.Elevator; -import frc4388.robot.subsystems.Endeffector; +// import frc4388.robot.subsystems.Endeffector; import frc4388.robot.subsystems.SwerveDrive; // Utilites @@ -74,8 +74,7 @@ public class RobotContainer { // private final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.camera); public final Lidar m_lidar = new Lidar(); - public final Elevator m_robotELevator= new Elevator(m_robotMap.elevator); - public final Endeffector m_robotEndeffector = new Endeffector(m_robotMap.endeffector); + public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endefectorLimitSwitch); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -214,7 +213,20 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); - // creates an empty command & requires the swerve drive, subsystems can run only 1 command at a time + + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionWaiting(), m_robotElevator)); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionReady(), m_robotElevator)); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionScoringThree(), m_robotElevator)); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotElevator.transitionScoringFour(), m_robotElevator)); + // ? /* Programer Buttons (Controller 3)*/ @@ -232,37 +244,6 @@ public class RobotContainer { true, false)) .onFalse(new InstantCommand()); - /*DPad for Level 1 and 2*/ - new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.9) - .onTrue(new InstantCommand(() -> m_robotELevator.PIDLevel2())) - .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop())); - - new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.9) - .onTrue(new InstantCommand(() -> m_robotELevator.PIDLevel1())) - .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop())); - - /*Free Brid Mode With Bummpers*/ - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotELevator.elevatorDown())) - .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop())); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotELevator.elevatorUp())) - .onFalse(new InstantCommand(() -> m_robotELevator.elevatorStop())); - - /*Endeffector Controls*/ - - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotEndeffector.PIDTop())) - .onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop())); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotEndeffector.PIDMiddle())) - .onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop())); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotEndeffector.PIDBottom())) - .onFalse(new InstantCommand(() -> m_robotEndeffector.endEffectorStop()));; } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index c939ca5..a5a630a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -18,8 +18,8 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import edu.wpi.first.wpilibj.DigitalInput; import frc4388.robot.Constants.ElevatorConstants; -import frc4388.robot.Constants.EndeffectorConstants; // import edu.wpi.first.wpilibj.motorcontrol.Spark; // import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; @@ -53,9 +53,11 @@ public class RobotMap { /* Elevator Subsystem */ public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); + 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); - /*Endeffector Subsystem*/ - public final TalonFX endeffector = new TalonFX(EndeffectorConstants.ENDEFFECTOR_ID.id); void configureDriveMotorControllers() {} } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index ef24876..5af17d3 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -9,52 +9,97 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ElevatorConstants; public class Elevator extends SubsystemBase { /** Creates a new Elevator. */ private TalonFX elevatorMotor; + private TalonFX endefectorMotor; - public Elevator(TalonFX elevatorTalonFX) { + private DigitalInput basinLimitSwitch; + private DigitalInput endefectorLimitSwitch; + + public enum CordinationState { + Waiting, // for coral into the though + Ready, // Has coral in enefector + ScoringThree, // Arm and elevator in the level three position + ScoringFour // Arm and elevator in the level four position + + } + + private CordinationState currentState; + + public Elevator(TalonFX elevatorTalonFX, TalonFX endefectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endefectorLimitSwitch) { elevatorMotor = elevatorTalonFX; + endefectorMotor = endefectorTalonFX; + + this.basinLimitSwitch = basinLimitSwitch; + this.endefectorLimitSwitch = endefectorLimitSwitch; elevatorMotor.setNeutralMode(NeutralModeValue.Brake); - + endefectorMotor.setNeutralMode(NeutralModeValue.Brake); elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); + endefectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFECTOR_PID); + currentState = CordinationState.Ready; } //PID methods - public void PIDPosition(double position) { + private void PIDPosition(TalonFX motor, double position) { var request = new PositionVoltage(position); elevatorMotor.setControl(request); } - public void PIDLevel1() { - PIDPosition(ElevatorConstants.LEVEL_1); - } - - public void PIDLevel2() { - PIDPosition(ElevatorConstants.LEVEL_2); - } - - public void elevatorUp() { - elevatorMotor.set(ElevatorConstants.ELEVATOR_SPEED_UP); - } - - public void elevatorDown() { - elevatorMotor.set(ElevatorConstants.ELEVATOR_SPEED_UP); - } - public void elevatorStop() { elevatorMotor.set(0); } + + public void endefectorStop() { + endefectorMotor.set(0); + } + public void transitionWaiting() { + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + currentState = CordinationState.Waiting; + } + + private void periodicWaiting() { + if (basinLimitSwitch.get()) transitionReady(); + } + + public void transitionReady() { + PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + currentState = CordinationState.Ready; + } + + public void transitionScoringThree() { + PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); + currentState = CordinationState.ScoringThree; + } + + private void periodicScoring() { + if (!endefectorLimitSwitch.get()) transitionWaiting(); + } + + public void transitionScoringFour() { + PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); + currentState = CordinationState.ScoringFour; + } @Override public void periodic() { // This method will be called once per scheduler run + if (currentState == CordinationState.Waiting) { + periodicWaiting(); + } else if (currentState == CordinationState.ScoringThree || currentState == CordinationState.ScoringFour) { + periodicScoring(); + } } } diff --git a/src/main/java/frc4388/robot/subsystems/Endeffector.java b/src/main/java/frc4388/robot/subsystems/Endeffector.java deleted file mode 100644 index ed7d3b5..0000000 --- a/src/main/java/frc4388/robot/subsystems/Endeffector.java +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.subsystems; - -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.ElevatorConstants; -import frc4388.robot.Constants.EndeffectorConstants; - -public class Endeffector extends SubsystemBase { - /** Creates a new Endefector. */ - private TalonFX endeffectorMotor; - public Endeffector(TalonFX endffectorTalonFX) { - endeffectorMotor = endffectorTalonFX; - - endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); - endeffectorMotor.getConfigurator().apply(EndeffectorConstants.ENDEFECTOR_PID); - - } - - public void PIDPosition(double position) { - var request = new PositionVoltage(position); - endeffectorMotor.setControl(request); - } - - public void PIDTop() { - PIDPosition(EndeffectorConstants.TOP); - } - - public void PIDMiddle() { - PIDPosition(EndeffectorConstants.MIDDLE); - } - - public void PIDBottom() { - PIDPosition(EndeffectorConstants.BOTTOM); - } - - public void endEffectorStop() { - endeffectorMotor.set(0); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} From 1bdba6d24bd57fab1d95ef0a3ca6247b39e03280 Mon Sep 17 00:00:00 2001 From: McGrathMaggie <78870mm@gmail.com> Date: Mon, 27 Jan 2025 19:35:08 -0700 Subject: [PATCH 2/2] Switch State Statement --- .../java/frc4388/robot/RobotContainer.java | 9 +-- .../frc4388/robot/subsystems/Elevator.java | 66 ++++++++++--------- 2 files changed, 41 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c4fd224..26b6185 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -47,6 +47,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; // Subsystems // import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Vision; +import frc4388.robot.subsystems.Elevator.CoordinationState; import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.Elevator; // import frc4388.robot.subsystems.Endeffector; @@ -216,16 +217,16 @@ public class RobotContainer { DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionWaiting(), m_robotElevator)); + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)); DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionReady(), m_robotElevator)); + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator)); DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionScoringThree(), m_robotElevator)); + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator)); DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionScoringFour(), m_robotElevator)); + .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator)); // ? /* Programer Buttons (Controller 3)*/ diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 5af17d3..f7fa030 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -21,7 +21,7 @@ public class Elevator extends SubsystemBase { private DigitalInput basinLimitSwitch; private DigitalInput endefectorLimitSwitch; - public enum CordinationState { + public enum CoordinationState { Waiting, // for coral into the though Ready, // Has coral in enefector ScoringThree, // Arm and elevator in the level three position @@ -29,7 +29,7 @@ public class Elevator extends SubsystemBase { } - private CordinationState currentState; + private CoordinationState currentState; public Elevator(TalonFX elevatorTalonFX, TalonFX endefectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endefectorLimitSwitch) { elevatorMotor = elevatorTalonFX; @@ -43,7 +43,7 @@ public class Elevator extends SubsystemBase { elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); endefectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFECTOR_PID); - currentState = CordinationState.Ready; + currentState = CoordinationState.Ready; } //PID methods @@ -60,45 +60,51 @@ public class Elevator extends SubsystemBase { public void endefectorStop() { endefectorMotor.set(0); } - - public void transitionWaiting() { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); - currentState = CordinationState.Waiting; + + public void transitionState(CoordinationState state) { + currentState = state; + switch (currentState) { + case Waiting: { + PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + break; + } + + case Ready: { + PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + break; + } + + case ScoringThree: { + PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); + break; + } + + case ScoringFour: { + PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); + break; + } + } + } private void periodicWaiting() { - if (basinLimitSwitch.get()) transitionReady(); - } - - public void transitionReady() { - PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); - currentState = CordinationState.Ready; - } - - public void transitionScoringThree() { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); - currentState = CordinationState.ScoringThree; + if (basinLimitSwitch.get()) transitionState(CoordinationState.Ready); } private void periodicScoring() { - if (!endefectorLimitSwitch.get()) transitionWaiting(); - } - - public void transitionScoringFour() { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); - currentState = CordinationState.ScoringFour; + if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting); } @Override public void periodic() { // This method will be called once per scheduler run - if (currentState == CordinationState.Waiting) { + if (currentState == CoordinationState.Waiting) { periodicWaiting(); - } else if (currentState == CordinationState.ScoringThree || currentState == CordinationState.ScoringFour) { + } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { periodicScoring(); } }