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

Fix limit switch and maybe autos

Browse Source
This commit is contained in:
Michael Mikovsky
2025-02-27 19:45:18 -07:00
parent 1b90bc8289
commit a063f4b309
5 changed files with 42 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+7
View File
@@ -377,6 +377,13 @@ public final class Constants {
public static final double L2_SCORE_DISTANCE = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
public static final int L4_DRIVE_TIME = 250; //Milliseconds
// public static final int L3_DRIVE_TIME = 500;
public static final int L2_DRIVE_TIME = 250; //Milliseconds
public static final int ALGAE_DRIVE_TIME = 500;
// Test april tag field layout
src/main/java/frc4388/robot/RobotContainer.java
+22 -22
View File
@@ -124,10 +124,10 @@ public class RobotContainer {
private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT)
)),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
// )),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
@@ -143,7 +143,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
@@ -152,10 +152,10 @@ public class RobotContainer {
private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT)
)),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
// )),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
@@ -172,7 +172,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
@@ -182,10 +182,10 @@ public class RobotContainer {
private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
// new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT)
)),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT),
// )),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -204,10 +204,10 @@ public class RobotContainer {
private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
// new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT)
)),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
// )),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
@@ -238,7 +238,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
@@ -253,7 +253,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -313,10 +313,10 @@ public class RobotContainer {
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL3Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL3Right);
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL2Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL2Right);
NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
@@ -659,9 +659,9 @@ public class RobotContainer {
// return autoCommand;
// } catch (Exception e) {
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
return autoCommand;
// return autoCommand;
// }
// return new PathPlannerAuto("Line-up-no-arm");
return new PathPlannerAuto("Line-up-no-arm");
// zach told me to do the below comment
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
src/main/java/frc4388/robot/commands/LidarAlign.java
+5 -2
View File
@@ -22,6 +22,7 @@ public class LidarAlign extends Command {
private boolean headedRight;
private double speed;
private int bounces;
private double additionalDistance = 0;
// private final boolean constructedHeadedRight;
/** Creates a new LidarAlign. */
@@ -41,6 +42,7 @@ public class LidarAlign extends Command {
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
this.additionalDistance = 0;
this.bounces = 0;
}
@@ -54,11 +56,12 @@ public class LidarAlign extends Command {
return;
}
if (currentFinderTick > 10) { //arbutrary threshhold for now.
if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
headedRight = !headedRight;
currentFinderTick *= -1;
bounces++;
if (bounces == 2) return;
additionalDistance += 5;
if (bounces == 5) return;
}
double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
src/main/java/frc4388/robot/subsystems/Elevator.java
+7 -2
View File
@@ -229,6 +229,8 @@ public class Elevator extends SubsystemBase {
private void periodicWaiting() {
if (!basinBeamBreak.get())
transitionState(CoordinationState.Ready);
if(!endeffectorLimitSwitch.get())
transitionState(CoordinationState.Hovering);
}
// private void periodicWaitingTripped() {
@@ -237,12 +239,15 @@ public class Elevator extends SubsystemBase {
// }
private void periodicReady() {
if (elevatorAtReference())
if (elevatorAtReference() && !endeffectorLimitSwitch.get())
transitionState(CoordinationState.Hovering);
if(elevatorAtReference() && endeffectorLimitSwitch.get())
transitionState(CoordinationState.Waiting);
}
private void periodicScoring() {
if (!endeffectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
if (!endeffectorLimitSwitch.get())
transitionState(CoordinationState.Waiting);
}
public void manualElevatorVel(double velocity) {