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/Constants.java
+2 -2
View File
@@ -419,7 +419,7 @@ public final class Constants {
//Max for elevator = 50%
public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
public static final double SCORING_THREE_ELEVATOR = -9.25;
public static final double DEALGAE_L2_ELEVATOR = -23.5;
@@ -437,7 +437,7 @@ public final class Constants {
public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
public static final double COMPLETLY_TOP_ENDEFECTOR = 0.51 * GEAR_RATIO_ENDEFECTOR;
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
.withKP(1)
src/main/java/frc4388/robot/RobotContainer.java
+12 -5
View File
@@ -141,19 +141,26 @@ public class RobotContainer {
private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
// new InstantCommand(() -> System.out.println("Soup")),
// new WaitCommand(1),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
);
@@ -529,7 +536,7 @@ public class RobotContainer {
// return autoCommand;
// } catch (Exception e) {
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
return Commands.none();
return autoCommand;
// }
// return new PathPlannerAuto("Line-up-no-arm");
// zach told me to do the below comment
@@ -551,9 +558,9 @@ public class RobotContainer {
autoChooser.onChange((filename) -> {
autoCommand = new PathPlannerAuto(filename);
System.out.println("Robot Auto Changed");
System.out.println("Robot Auto Changed " + filename);
});
SmartDashboard.putData(autoChooser);
// SmartDashboard.putData(autoChooser);
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+14 -9
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) {