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

de stall l4

Browse Source
This commit is contained in:
C4llSiqn
2025-02-25 11:33:46 -07:00
parent 761a56cf0f
commit f9eb029b66
3 changed files with 29 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Elevator.java
+15 -10
View File
@@ -178,20 +178,25 @@ public class Elevator extends SubsystemBase {
public boolean elevatorAtRefrence() {
// double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.5;
if (atPos) {
SmartDashboard.putNumber("Elevator Refrence", elevatorRefrence);
SmartDashboard.putNumber("Elevator Pos", elevatorPosition);
}
double diffrence = Math.abs(elevatorPosition) - Math.abs(elevatorRefrence);
return atPos;
boolean headedUp = diffrence < 0;
boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
return (Math.abs(diffrence) >= 99999 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
}
public boolean endefectorAtRefrence() {
// double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
double diffrence = Math.abs(endefectorPosition) - Math.abs(endefectorRefrence);
return Math.abs(endefectorRefrence - endefectorPosition) <= 0.2;
boolean headedUp = diffrence < 0;
boolean forwardLimit = endefectorMotor.getForwardLimit().asSupplier().get().value == 0;
boolean reverseLimit = endefectorMotor.getReverseLimit().asSupplier().get().value == 0;
return (Math.abs(diffrence) >= 99999 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
}
// public void driveElevatorStick(Translation2d stick) {
// if (stick.getNorm() > 0.05) {
@@ -210,7 +215,7 @@ public class Elevator extends SubsystemBase {
// }
private void periodicReady() {
if (elevatorAtRefrence())
if (elevatorMotor.getForwardLimit().asSupplier().get().value == 0)
transitionState(CoordinationState.Hovering);
}
@@ -227,7 +232,7 @@ public class Elevator extends SubsystemBase {
SmartDashboard.putString("State", currentState.toString());
if (currentState == CoordinationState.Waiting) {
// periodicWaiting();
periodicWaiting();
} else if (currentState == CoordinationState.WatingBeamTriped) {
// periodicWaitingTripped();
} else if (currentState == CoordinationState.Ready) {