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

Preferred operator controls

Browse Source 
Added preferred operator controls
Added manual controls
Renamed misspelled items
This commit is contained in:
McGrathMaggie
2025-02-25 19:47:57 -07:00
parent ddbae68851
commit b341acaa3d
6 changed files with 69 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Elevator.java
+42 -34
View File
@@ -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();