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

Intergrate endefector into elevator for cordination

Browse Source
This commit is contained in:
C4llSiqn
2025-01-27 17:29:25 -07:00
parent a7ac44c25d
commit 2b376d741c
5 changed files with 101 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Elevator.java
+64 -19
View File
@@ -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();
}
}
}