Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Switch State Statement

Browse Source
This commit is contained in:
McGrathMaggie
2025-01-27 19:35:08 -07:00
parent fd63bceaeb
commit 1bdba6d24b
2 changed files with 41 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+5 -4
View File
@@ -47,6 +47,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
// Subsystems // Subsystems
// import frc4388.robot.subsystems.LED; // import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Vision;
import frc4388.robot.subsystems.Elevator.CoordinationState;
import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.Lidar;
import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Elevator;
// import frc4388.robot.subsystems.Endeffector; // import frc4388.robot.subsystems.Endeffector;
@@ -216,16 +217,16 @@ public class RobotContainer {
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) 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) 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) 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) 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)*/ // ? /* Programer Buttons (Controller 3)*/
src/main/java/frc4388/robot/subsystems/Elevator.java
+36 -30
View File
@@ -21,7 +21,7 @@ public class Elevator extends SubsystemBase {
private DigitalInput basinLimitSwitch; private DigitalInput basinLimitSwitch;
private DigitalInput endefectorLimitSwitch; private DigitalInput endefectorLimitSwitch;
public enum CordinationState { public enum CoordinationState {
Waiting, // for coral into the though Waiting, // for coral into the though
Ready, // Has coral in enefector Ready, // Has coral in enefector
ScoringThree, // Arm and elevator in the level three position 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) { public Elevator(TalonFX elevatorTalonFX, TalonFX endefectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endefectorLimitSwitch) {
elevatorMotor = elevatorTalonFX; elevatorMotor = elevatorTalonFX;
@@ -43,7 +43,7 @@ public class Elevator extends SubsystemBase {
elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
endefectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFECTOR_PID); endefectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFECTOR_PID);
currentState = CordinationState.Ready; currentState = CoordinationState.Ready;
} }
//PID methods //PID methods
@@ -60,45 +60,51 @@ public class Elevator extends SubsystemBase {
public void endefectorStop() { public void endefectorStop() {
endefectorMotor.set(0); endefectorMotor.set(0);
} }
public void transitionWaiting() { public void transitionState(CoordinationState state) {
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); currentState = state;
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); switch (currentState) {
currentState = CordinationState.Waiting; 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() { private void periodicWaiting() {
if (basinLimitSwitch.get()) transitionReady(); if (basinLimitSwitch.get()) transitionState(CoordinationState.Ready);
}
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() { private void periodicScoring() {
if (!endefectorLimitSwitch.get()) transitionWaiting(); if (!endefectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
}
public void transitionScoringFour() {
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR);
PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR);
currentState = CordinationState.ScoringFour;
} }
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run // This method will be called once per scheduler run
if (currentState == CordinationState.Waiting) { if (currentState == CoordinationState.Waiting) {
periodicWaiting(); periodicWaiting();
} else if (currentState == CordinationState.ScoringThree || currentState == CordinationState.ScoringFour) { } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
periodicScoring(); periodicScoring();
} }
} }