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

Merge branch 'master' into driver-preference-controls

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-26 17:11:37 -07:00
parent e7c59b7d58 46674cc213
commit 30f5c0df63
15 changed files with 275 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Elevator.java
+21 -7
View File
@@ -181,20 +181,26 @@ public class Elevator extends SubsystemBase {
public boolean elevatorAtReference() {
// 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 = elevatorRefrence - elevatorPosition;
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) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
}
public boolean endeffectorAtReference() {
// double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
double diffrence = endefectorRefrence - endefectorPosition;
return Math.abs(endeffectorRefrence - endeffectorPosition) <= 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) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
}
// public void driveElevatorStick(Translation2d stick) {
// if (stick.getNorm() > 0.05) {
@@ -261,6 +267,14 @@ public class Elevator extends SubsystemBase {
// }
}
public boolean isL4Primed(){
return currentState == CoordinationState.PrimedFour;
}
public boolean isL3Primed(){
return currentState == CoordinationState.PrimedThree;
}
public void armShuffle(){
if(!basinBeamBreak.get()){
//shuffle the coral with the arm until coral hits beam break