From 8bea59723078dfda9653a8fda0e8ddb62e557c94 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 3 Mar 2025 17:49:06 -0700 Subject: [PATCH] i --- .../pathplanner/autos/2 Coral Cage 1.auto | 12 +++++++ .../pathplanner/autos/2 Coral Cage 5.auto | 12 +++++++ src/main/java/frc4388/robot/Constants.java | 2 ++ .../java/frc4388/robot/RobotContainer.java | 15 +++++--- src/main/java/frc4388/robot/RobotMap.java | 1 + .../frc4388/robot/commands/waitSupplier.java | 36 +++++++++++++++++++ .../frc4388/robot/subsystems/Elevator.java | 20 +++++++---- 7 files changed, 86 insertions(+), 12 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/waitSupplier.java diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto index e35d8f0..fd705bb 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { @@ -40,6 +46,12 @@ "pathName": "Top Feed to Reef 6" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto index 387787a..c538e81 100644 --- a/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { @@ -40,6 +46,12 @@ "pathName": "Bottom Feed to Reef 2" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 70df5bc..731bb76 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -427,6 +427,8 @@ public final class Constants { public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND + public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND + public static final double GEAR_RATIO_ELEVATOR = -9.0; //Max for elevator = 50% diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 982e1a7..990fd31 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -49,6 +49,7 @@ import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.waitElevatorRefrence; import frc4388.robot.commands.waitEndefectorRefrence; import frc4388.robot.commands.waitFeedCoral; +import frc4388.robot.commands.waitSupplier; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -89,7 +90,7 @@ public class RobotContainer { public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); - public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_vision, m_robotLED); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.basinLimitSwitch, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -108,7 +109,7 @@ public class RobotContainer { // ! Teleop Commands public void stop() { - new InstantCommand(()->{}, m_robotSwerveDrive).schedule();;;;; + new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); m_robotSwerveDrive.stopModules(); } @@ -117,6 +118,8 @@ public class RobotContainer { private SendableChooser autoChooser; private Command autoCommand; + private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove); + // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, // () -> autoplaybackName.get(), // lastAutoName @@ -316,7 +319,9 @@ public class RobotContainer { */ public RobotContainer() { - NamedCommands.registerCommand("grab-coral", new waitFeedCoral(m_robotElevator)); + NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy()); + NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); + NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0, 1), @@ -330,8 +335,8 @@ public class RobotContainer { NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right); - NamedCommands.registerCommand("enable-preraise-l4", new InstantCommand(() -> { - m_robotElevator.enablePreRaiseElevator(); + NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> { + m_robotElevator.transitionState(CoordinationState.PrimedFour); })); configureButtonBindings(); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 0313a70..72bafdd 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -58,6 +58,7 @@ public class RobotMap { public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); + public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); void configureDriveMotorControllers() {} diff --git a/src/main/java/frc4388/robot/commands/waitSupplier.java b/src/main/java/frc4388/robot/commands/waitSupplier.java new file mode 100644 index 0000000..0ea5a3b --- /dev/null +++ b/src/main/java/frc4388/robot/commands/waitSupplier.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitSupplier extends Command { + /** Creates a new waitSupplier. */ + private final Supplier truth; + public waitSupplier(Supplier truth) { + this.truth = truth; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return truth.get(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 06162a4..57d119c 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -49,7 +49,7 @@ public class Elevator extends Subsystem { private DigitalInput basinBeamBreak; private DigitalInput endeffectorLimitSwitch; - // private DigitalInput intakeIR; + private DigitalInput intakeIR; public enum CoordinationState { Waiting, // for coral into the though @@ -69,15 +69,15 @@ public class Elevator extends Subsystem { private CoordinationState currentState; - public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { - // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { + // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { + public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { elevatorMotor = elevatorTalonFX; endeffectorMotor = endeffectorTalonFX; this.led = led; this.basinBeamBreak = basinLimitSwitch; this.endeffectorLimitSwitch = endeffectorLimitSwitch; - // this.intakeIR = intakeDigitalInput; + this.intakeIR = intakeDigitalInput; elevatorMotor.setNeutralMode(NeutralModeValue.Brake); endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); @@ -294,6 +294,7 @@ public class Elevator extends Subsystem { // This method will be called once per scheduler run SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); + SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); SmartDashboard.putString("State", currentState.toString()); if (disableAutoIntake) return; @@ -310,18 +311,23 @@ public class Elevator extends Subsystem { // } } - public boolean isL4Primed(){ + public boolean isL4Primed() { return currentState == CoordinationState.PrimedFour; } - public boolean isL3Primed(){ + public boolean isL3Primed() { return currentState == CoordinationState.PrimedThree; } - public boolean hasCoral(){ + public boolean hasCoral() { return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get(); } + public boolean readyToMove() { + // return !intakeIR.get() || hasCoral(); + return hasCoral(); + } + public void armShuffle(){ if(!basinBeamBreak.get()){ //shuffle the coral with the arm until coral hits beam break